From 04a3c4af20df9e851e281776bfa51d02759ddee6 Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Tue, 18 Feb 2025 18:26:38 +0100 Subject: [PATCH] Differential Rover: Refactor and clean up, align with Ackermann rover(#24318) * differential: refactor code architecture * Offboard fix * fix accel/decel slew rate --- .../init.d-posix/airframes/4009_gz_r1_rover | 44 +- .../airframes/50001_aion_robotics_r1_rover | 43 +- msg/CMakeLists.txt | 3 - msg/RoverDifferentialGuidanceStatus.msg | 7 - msg/RoverDifferentialSetpoint.msg | 9 - msg/RoverDifferentialStatus.msg | 14 - src/lib/rover_control/RoverControl.cpp | 89 +++- src/lib/rover_control/RoverControl.hpp | 19 + src/lib/rover_control/module.yaml | 18 +- src/modules/logger/logged_topics.cpp | 3 - src/modules/rover_ackermann/Kconfig | 2 +- src/modules/rover_differential/CMakeLists.txt | 14 +- .../CMakeLists.txt | 10 +- .../DifferentialAttControl.cpp | 208 +++++++++ .../DifferentialAttControl.hpp | 143 ++++++ .../CMakeLists.txt | 9 +- .../DifferentialPosVelControl.cpp | 406 ++++++++++++++++++ .../DifferentialPosVelControl.hpp | 243 +++++++++++ .../DifferentialRateControl/CMakeLists.txt | 39 ++ .../DifferentialRateControl.cpp | 190 ++++++++ .../DifferentialRateControl.hpp | 144 +++++++ src/modules/rover_differential/Kconfig | 3 +- .../rover_differential/RoverDifferential.cpp | 283 ++++-------- .../rover_differential/RoverDifferential.hpp | 119 ++--- .../RoverDifferentialControl.cpp | 277 ------------ .../RoverDifferentialControl.hpp | 169 -------- .../RoverDifferentialGuidance.cpp | 234 ---------- .../RoverDifferentialGuidance.hpp | 151 ------- src/modules/rover_differential/module.yaml | 191 +------- 29 files changed, 1722 insertions(+), 1362 deletions(-) delete mode 100644 msg/RoverDifferentialGuidanceStatus.msg delete mode 100644 msg/RoverDifferentialSetpoint.msg delete mode 100644 msg/RoverDifferentialStatus.msg rename src/modules/rover_differential/{RoverDifferentialControl => DifferentialAttControl}/CMakeLists.txt (85%) create mode 100644 src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp create mode 100644 src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp rename src/modules/rover_differential/{RoverDifferentialGuidance => DifferentialPosVelControl}/CMakeLists.txt (86%) create mode 100644 src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp create mode 100644 src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp create mode 100644 src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt create mode 100644 src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp create mode 100644 src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp delete mode 100644 src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp delete mode 100644 src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp delete mode 100644 src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp delete mode 100644 src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 3f96dad5f8..f05bbdcf45 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -11,29 +11,39 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Rover parameters +param set-default NAV_ACC_RAD 0.5 + +# Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAX_ACCEL 5 -param set-default RD_MAX_DECEL 10 -param set-default RD_MAX_JERK 30 param set-default RD_MAX_THR_YAW_R 1.5 -param set-default RD_YAW_RATE_P 0.25 -param set-default RD_YAW_RATE_I 0.01 -param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0.1 -param set-default RD_MAX_SPEED 2 -param set-default RD_MAX_THR_SPD 2.15 -param set-default RD_SPEED_P 0.1 -param set-default RD_SPEED_I 0.01 -param set-default RD_MAX_YAW_RATE 180 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 -param set-default RD_MAX_YAW_ACCEL 1000 -# Pure pursuit parameters -param set-default PP_LOOKAHD_MAX 30 -param set-default PP_LOOKAHD_MIN 2 +# Rover Control Parameters +param set-default RO_ACCEL_LIM 5 +param set-default RO_DECEL_LIM 10 +param set-default RO_JERK_LIM 30 +param set-default RO_MAX_THR_SPEED 2.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.25 +param set-default RO_YAW_RATE_LIM 180 +param set-default RO_YAW_ACCEL_LIM 120 +param set-default RO_YAW_DECEL_LIM 1000 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 2 +param set-default RO_SPEED_I 0.5 +param set-default RO_SPEED_P 1 + +# Pure Pursuit parameters param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1 # Simulated sensors param set-default SENS_EN_GPSSIM 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 57a3644dae..45b35ffeaa 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -21,26 +21,37 @@ param set-default RBCLW_FUNC1 101 param set-default RBCLW_FUNC2 102 param set-default RBCLW_REV 1 # reverse right wheels -# Rover parameters + +param set-default NAV_ACC_RAD 0.5 + +# Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAX_ACCEL 3 -param set-default RD_MAX_DECEL 4 -param set-default RD_MAX_JERK 5 -param set-default RD_MAX_SPEED 1.6 -param set-default RD_MAX_THR_SPD 1.9 param set-default RD_MAX_THR_YAW_R 0.7 -param set-default RD_MAX_YAW_ACCEL 600 -param set-default RD_MAX_YAW_RATE 250 -param set-default RD_SPEED_P 0.1 -param set-default RD_SPEED_I 0.01 param set-default RD_TRANS_DRV_TRN 0.785398 param set-default RD_TRANS_TRN_DRV 0.139626 -param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0.1 -param set-default RD_YAW_RATE_P 0.1 -param set-default RD_YAW_RATE_I 0.01 -# Pure pursuit parameters +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 4 +param set-default RO_JERK_LIM 5 +param set-default RO_MAX_THR_SPEED 1.9 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.1 +param set-default RO_YAW_RATE_LIM 250 +param set-default RO_YAW_ACCEL_LIM 600 +param set-default RO_YAW_DECEL_LIM 600 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 1.6 +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 param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 -param set-default PP_LOOKAHD_GAIN 1 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 5844caafec..ac59212f40 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -179,9 +179,6 @@ set(msg_files RoverRateStatus.msg RoverSteeringSetpoint.msg RoverThrottleSetpoint.msg - RoverDifferentialGuidanceStatus.msg - RoverDifferentialSetpoint.msg - RoverDifferentialStatus.msg RoverMecanumGuidanceStatus.msg RoverMecanumSetpoint.msg RoverMecanumStatus.msg diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg deleted file mode 100644 index ce3d375111..0000000000 --- a/msg/RoverDifferentialGuidanceStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error_deg # [deg] Heading error of the pure pursuit controller -uint8 state_machine # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED] - -# TOPICS rover_differential_guidance_status diff --git a/msg/RoverDifferentialSetpoint.msg b/msg/RoverDifferentialSetpoint.msg deleted file mode 100644 index 100cc6b3cd..0000000000 --- a/msg/RoverDifferentialSetpoint.msg +++ /dev/null @@ -1,9 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 forward_speed_setpoint # [m/s] Desired forward speed for the rover -float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover -float32 yaw_rate_setpoint # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used) -float32 speed_diff_setpoint_normalized # [-1, 1] Normalized speed difference between the left and right wheels -float32 yaw_setpoint # [rad] Desired yaw (heading) for the rover - -# TOPICS rover_differential_setpoint diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg deleted file mode 100644 index 767474fb5b..0000000000 --- a/msg/RoverDifferentialStatus.msg +++ /dev/null @@ -1,14 +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 measured_yaw # [rad] Measured yaw -float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate -float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller -float32 measured_yaw_rate # [rad/s] Measured yaw rate -float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw controller -float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller -float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller - -# TOPICS rover_differential_status diff --git a/src/lib/rover_control/RoverControl.cpp b/src/lib/rover_control/RoverControl.cpp index 7b0864a428..0eafcaa9f8 100644 --- a/src/lib/rover_control/RoverControl.cpp +++ b/src/lib/rover_control/RoverControl.cpp @@ -48,26 +48,24 @@ float throttleControl(SlewRate &motor_setpoint, const float throttle_setp if (accelerating && max_accel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Acceleration slew rate motor_setpoint.setSlewRate(max_accel / max_thr_spd); + motor_setpoint.update(throttle_setpoint, dt); // 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.setForcedValue(throttle_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); + motor_setpoint.update(throttle_setpoint, dt); // 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.setForcedValue(throttle_setpoint); } - motor_setpoint.update(throttle_setpoint, dt); - } else { // Fallthrough if slew rate parameters are not configured motor_setpoint.setForcedValue(throttle_setpoint); } @@ -88,14 +86,13 @@ float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, if (yaw_slew_rate > FLT_EPSILON) { // Apply slew rate if configured adjusted_yaw_setpoint.setSlewRate(yaw_slew_rate); + adjusted_yaw_setpoint.update(yaw_setpoint, dt); // 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.setForcedValue(yaw_setpoint); } - adjusted_yaw_setpoint.update(yaw_setpoint, dt); - } else { adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); } @@ -118,26 +115,24 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const // Apply acceleration and deceleration limit if (fabsf(speed_setpoint) >= fabsf(vehicle_speed) && max_accel > FLT_EPSILON) { speed_with_rate_limit.setSlewRate(max_accel); + speed_with_rate_limit.update(speed_setpoint, dt); // 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.setForcedValue(speed_setpoint); } - 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); + speed_with_rate_limit.update(speed_setpoint, dt); // 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.setForcedValue(speed_setpoint); } - speed_with_rate_limit.update(speed_setpoint, dt); - } else { // Fallthrough if slew rate is not configured speed_with_rate_limit.setForcedValue(speed_setpoint); } @@ -153,12 +148,72 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const } // Feedback control - pid_speed.setSetpoint(speed_with_rate_limit.getState()); - forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + if (fabsf(speed_with_rate_limit.getState()) > FLT_EPSILON) { + pid_speed.setSetpoint(speed_with_rate_limit.getState()); + forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + + } else { + pid_speed.resetIntegral(); + } + return math::constrain(forward_speed_normalized, -1.f, 1.f); } +float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint, + const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel, + const float wheel_track, const float dt) +{ + // Apply acceleration and deceleration limit + if (fabsf(yaw_rate_setpoint) >= fabsf(vehicle_yaw_rate) && max_yaw_accel > FLT_EPSILON) { + adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_accel); + adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf( + yaw_rate_setpoint - vehicle_yaw_rate)) { + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + + } else if (fabsf(yaw_rate_setpoint) < fabsf(vehicle_yaw_rate) && max_yaw_decel > FLT_EPSILON) { + adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_decel); + adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf( + yaw_rate_setpoint - vehicle_yaw_rate)) { + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + + } else { // Fallthrough if slew rate is not configured + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + // Transform yaw rate into speed difference + float speed_diff_normalized{0.f}; + + if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward + const float speed_diff = adjusted_yaw_rate_setpoint.getState() * wheel_track / + 2.f; + speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, + max_thr_yaw_r, -1.f, 1.f); + } + + // Feedback control + if (fabsf(adjusted_yaw_rate_setpoint.getState()) > FLT_EPSILON) { + pid_yaw_rate.setSetpoint(adjusted_yaw_rate_setpoint.getState()); + speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); + + } else { + pid_yaw_rate.resetIntegral(); + } + + + return math::constrain(speed_diff_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) diff --git a/src/lib/rover_control/RoverControl.hpp b/src/lib/rover_control/RoverControl.hpp index 7f76710225..1196e04650 100644 --- a/src/lib/rover_control/RoverControl.hpp +++ b/src/lib/rover_control/RoverControl.hpp @@ -92,6 +92,25 @@ float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, f 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); +/** + * Applies yaw acceleration slew rate to a yaw rate setpoint and calculates the necessary speed diff setpoint + * using a feedforward term and/or a PID controller. + * Note: This function is only for rovers that control the rate through a speed difference between the left/right wheels. + * @param adjusted_yaw_rate_setpoint Yaw rate setpoint with applied slew rate [-1, 1] (Updated by this function). + * @param pid_yaw_rate Yaw rate PID (Updated by this function). + * @param yaw_rate_setpoint Yaw rate setpoint [rad/s]. + * @param vehicle_yaw_rate Measured vehicle yaw rate [rad/s]. + * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. + * @param max_yaw_accel Maximum allowed yaw acceleration [rad/s^2]. + * @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2]. + * @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m]. + * @param dt Time since last update [s]. + * @return Normalized speed difference setpoint [-1, 1]. + */ +float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, + float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel, + float wheel_track, float dt); + /** * Projects positionSetpointTriplet waypoints from global to ned frame. * @param curr_wp_ned Current waypoint in NED frame (Updated by this function) diff --git a/src/lib/rover_control/module.yaml b/src/lib/rover_control/module.yaml index 8450a03b06..a786a32c4b 100644 --- a/src/lib/rover_control/module.yaml +++ b/src/lib/rover_control/module.yaml @@ -133,7 +133,23 @@ parameters: RO_YAW_ACCEL_LIM: description: short: Yaw acceleration limit - long: Set to -1 to disable. + long: | + Used to cap how quickly the magnitude of yaw rate setpoints can increase. + Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 10000 + increment: 0.01 + decimal: 2 + default: -1 + + RO_YAW_DECEL_LIM: + description: + short: Yaw deceleration limit + long: | + Used to cap how quickly the magnitude of yaw rate setpoints can decrease. + Set to -1 to disable. type: float unit: deg/s^2 min: -1 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 4882061d1d..b98d185aff 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -110,9 +110,6 @@ void LoggedTopics::add_default_topics() 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); add_optional_topic("rover_mecanum_guidance_status", 100); add_optional_topic("rover_mecanum_setpoint", 100); add_optional_topic("rover_mecanum_status", 100); diff --git a/src/modules/rover_ackermann/Kconfig b/src/modules/rover_ackermann/Kconfig index a6acd00251..bfcff4a895 100644 --- a/src/modules/rover_ackermann/Kconfig +++ b/src/modules/rover_ackermann/Kconfig @@ -2,4 +2,4 @@ menuconfig MODULES_ROVER_ACKERMANN bool "rover_ackermann" default n ---help--- - Enable support for rover_ackermann + Enable support for ackermann rovers diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index 8805893160..8fafc30bab 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/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(RoverDifferentialGuidance) -add_subdirectory(RoverDifferentialControl) +add_subdirectory(DifferentialRateControl) +add_subdirectory(DifferentialAttControl) +add_subdirectory(DifferentialPosVelControl) px4_add_module( MODULE modules__rover_differential @@ -41,10 +42,11 @@ px4_add_module( RoverDifferential.cpp RoverDifferential.hpp DEPENDS - RoverDifferentialGuidance - RoverDifferentialControl + DifferentialRateControl + DifferentialAttControl + DifferentialPosVelControl px4_work_queue - pure_pursuit + rover_control MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt similarity index 85% rename from src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt rename to src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt index 7464de4f5b..456b21489d 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt +++ b/src/modules/rover_differential/DifferentialAttControl/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(RoverDifferentialControl - RoverDifferentialControl.cpp +px4_add_library(DifferentialAttControl + DifferentialAttControl.cpp ) -target_link_libraries(RoverDifferentialControl PUBLIC PID) -target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(DifferentialAttControl PUBLIC PID) +target_include_directories(DifferentialAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp new file mode 100644 index 0000000000..a36ed41ada --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * 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 "DifferentialAttControl.hpp" + +using namespace time_literals; + +DifferentialAttControl::DifferentialAttControl(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 DifferentialAttControl::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(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void DifferentialAttControl::updateAttControl() +{ + const 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()) { + + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_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 DifferentialAttControl::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); + + 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(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 = 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); + } + + + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_att_control = _offboard_control_mode.attitude && !_offboard_control_mode.position + && !_offboard_control_mode.velocity; + + if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void DifferentialAttControl::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; + } + + const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_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 DifferentialAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_yaw_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_att_control_conf_invalid_yaw_p"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp new file mode 100644 index 0000000000..e4198384c0 --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for differential attitude control. + */ +class DifferentialAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialAttControl. + * @param parent The parent ModuleParams object. + */ + DifferentialAttControl(ModuleParams *parent); + ~DifferentialAttControl() = 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) + * or trajectorySetpoint (Offboard attitude control). + */ + 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 _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 _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{}; + 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_attitude_status_pub{ORB_ID(rover_attitude_status)}; + + // Variables + hrt_abstime _timestamp{0}; + hrt_abstime _last_rate_setpoint_update{0}; + float _vehicle_yaw{0.f}; + float _dt{0.f}; + float _max_yaw_rate{0.f}; + 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 + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz + ) +}; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt b/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt similarity index 86% rename from src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt rename to src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt index 81f350d484..a4a4795be7 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt +++ b/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2023-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 @@ # ############################################################################ -px4_add_library(RoverDifferentialGuidance - RoverDifferentialGuidance.cpp +px4_add_library(DifferentialPosVelControl + DifferentialPosVelControl.cpp ) -target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(DifferentialPosVelControl PUBLIC PID) +target_include_directories(DifferentialPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp new file mode 100644 index 0000000000..32532f4c56 --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp @@ -0,0 +1,406 @@ +/**************************************************************************** + * + * 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 "DifferentialPosVelControl.hpp" + +using namespace time_literals; + +DifferentialPosVelControl::DifferentialPosVelControl(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 DifferentialPosVelControl::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()); + } +} + +void DifferentialPosVelControl::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(); + + if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { + + const float speed_body_x_setpoint_normalized = math::interpolate(_speed_body_x_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + if (fabsf(speed_body_x_setpoint_normalized) > 1.f - + fabsf(_rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels + _speed_body_x_setpoint = sign(_speed_body_x_setpoint) * + math::interpolate(1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff), -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + } + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + _speed_body_x_setpoint = fabsf(_speed_body_x_setpoint) > _param_ro_speed_th.get() ? _speed_body_x_setpoint : 0.f; + 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 DifferentialPosVelControl::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); + const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + +} + +void DifferentialPosVelControl::generateAttitudeSetpoint() +{ + if (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + if (_offboard_control_mode.position) { + offboardPositionMode(); + + } else if (_offboard_control_mode.velocity) { + offboardVelocityMode(); + } + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode + autoPositionMode(); + } +} + +void DifferentialPosVelControl::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 = 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 DifferentialPosVelControl::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 DifferentialPosVelControl::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 DifferentialPosVelControl::autoPositionMode() +{ + updateAutoSubscriptions(); + + // Distances to waypoints + 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(); + } + + // State machine + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + fabsf(_vehicle_speed_body_x)); + const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw); + + if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { + if (_currentState == GuidanceState::STOPPED) { + _currentState = GuidanceState::DRIVING; + } + + if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { + _currentState = GuidanceState::SPOT_TURNING; + + } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + _currentState = GuidanceState::DRIVING; + } + + } else { // Mission finished or delay command + _currentState = GuidanceState::STOPPED; + } + + // Guidance logic + switch (_currentState) { + case GuidanceState::DRIVING: { + // Calculate desired speed in body x direction + _speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), + _param_rd_miss_spd_gain.get()); + + } break; + + case GuidanceState::SPOT_TURNING: + if (fabsf(_vehicle_speed_body_x) > 0.f) { + yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop + + } + + _speed_body_x_setpoint = 0.f; + break; + + case GuidanceState::STOPPED: + default: + yaw_setpoint = _vehicle_yaw; + _speed_body_x_setpoint = 0.f; + break; + + } + + 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 DifferentialPosVelControl::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()) { + 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); + + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // 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(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +float DifferentialPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp, + const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, + const float trans_drv_trn, const float miss_spd_gain) +{ + float speed_body_x_setpoint = cruising_speed; + + if (_waypoint_transition_angle < M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON) { + speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, 0.0f); + + } else if (_waypoint_transition_angle >= M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON + && miss_spd_gain > FLT_EPSILON) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle, + 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); + speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, + max_speed * (1.f - speed_reduction)); + } + + return math::constrain(speed_body_x_setpoint, -cruising_speed, cruising_speed); + +} + +bool DifferentialPosVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + } + + } + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_posVel_control_conf_invalid_speed_control"), events::Log::Error, + "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup", + _param_ro_max_thr_speed.get(), + _param_ro_speed_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp new file mode 100644 index 0000000000..0ae8a24eae --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp @@ -0,0 +1,243 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + SPOT_TURNING, // The vehicle is currently turning on the spot. + DRIVING, // The vehicle is currently driving. + STOPPED // The vehicle is stopped. +}; + +/** + * @brief Class for differential position/velocity control. + */ +class DifferentialPosVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialPosVelControl. + * @param parent The parent ModuleParams object. + */ + DifferentialPosVelControl(ModuleParams *parent); + ~DifferentialPosVelControl() = 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 Calculate the speed setpoint. During waypoint transition the speed is restricted to + * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). + * On straight lines it is based on a speed trajectory such that the rover will arrive at the next waypoint transition + * with the desired waypoiny transition speed under consideration of the maximum deceleration and jerk. + * @param cruising_speed Cruising speed [m/s]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param max_decel Maximum allowed deceleration [m/s^2]. + * @param max_jerk Maximum allowed jerk [m/s^3]. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum speed setpoint [m/s] + * @param trans_drv_trn Heading error threshold to switch from driving to turning [rad]. + * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. + * @return Speed setpoint [m/s]. + */ + float calcSpeedSetpoint(float cruising_speed, float distance_to_curr_wp, float max_decel, float max_jerk, + float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain); + + + /** + * @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)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + + // uORB publications + uORB::Publication _rover_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 _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}; + + // Waypoint variables + Vector2d _home_position{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _cruising_speed{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + bool _prev_param_check_passed{true}; + + // 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 + GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. + + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn, + (ParamFloat) _param_rd_miss_spd_gain, + (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_nav_acc_rad + + ) +}; diff --git a/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt new file mode 100644 index 0000000000..0182e89d30 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/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(DifferentialRateControl + DifferentialRateControl.cpp +) + +target_link_libraries(DifferentialRateControl PUBLIC PID) +target_include_directories(DifferentialRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp new file mode 100644 index 0000000000..fe62328cd9 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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 "DifferentialRateControl.hpp" + +using namespace time_literals; + +DifferentialRateControl::DifferentialRateControl(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 DifferentialRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_accel = _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_decel = _param_ro_yaw_decel_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); + _adjusted_yaw_rate_setpoint.setSlewRate(_max_yaw_accel); +} + +void DifferentialRateControl::updateRateControl() +{ + const 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()) { + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateRateSetpoint(); + } + + generateSteeringSetpoint(); + + } else { // Reset controller and slew rate when rate control is not active + _pid_yaw_rate.resetIntegral(); + _adjusted_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 = _adjusted_yaw_rate_setpoint.getState(); + rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_rate_status_pub.publish(rover_rate_status); + +} + +void DifferentialRateControl::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 = math::interpolate (manual_control_setpoint.roll, -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard rate control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.position + && !_offboard_control_mode.velocity && !_offboard_control_mode.attitude; + + if (offboard_rate_control && PX4_ISFINITE(trajectory_setpoint.yawspeed)) { + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + } +} + +void DifferentialRateControl::generateSteeringSetpoint() +{ + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + + } + + float speed_diff_normalized{0.f}; + + if (PX4_ISFINITE(_rover_rate_setpoint.yaw_rate_setpoint) && PX4_ISFINITE(_vehicle_yaw_rate)) { + const float yaw_rate_setpoint = fabsf(_rover_rate_setpoint.yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * + M_DEG_TO_RAD_F ? + _rover_rate_setpoint.yaw_rate_setpoint : 0.f; + speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, + _max_yaw_decel, _param_rd_wheel_track.get(), _dt); + } + + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); +} + +bool DifferentialRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_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()); + } + + } + + if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_rd_max_thr_yaw_r.get() < FLT_EPSILON) + && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_rate_control_conf_invalid_rate_control"), events::Log::Error, + "Invalid configuration for rate control: Neither feed forward nor feedback is setup", _param_rd_wheel_track.get(), + _param_rd_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp new file mode 100644 index 0000000000..5b9bd6d632 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 +#include +#include + +/** + * @brief Class for differential rate control. + */ +class DifferentialRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialRateControl. + * @param parent The parent ModuleParams object. + */ + DifferentialRateControl(ModuleParams *parent); + ~DifferentialRateControl() = 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) + * or trajectorySetpoint (Offboard rate control). + */ + 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 _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + 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{}; + offboard_control_mode_s _offboard_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 + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; + float _max_yaw_decel{0.f}; + float _vehicle_yaw_rate{0.f}; + float _dt{0.f}; // Time since last update [s]. + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _adjusted_yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_max_thr_yaw_r, + (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_yaw_decel_limit + ) +}; diff --git a/src/modules/rover_differential/Kconfig b/src/modules/rover_differential/Kconfig index 840e2cdbf9..ed62f619b7 100644 --- a/src/modules/rover_differential/Kconfig +++ b/src/modules/rover_differential/Kconfig @@ -1,6 +1,5 @@ menuconfig MODULES_ROVER_DIFFERENTIAL bool "rover_differential" default n - depends on MODULES_CONTROL_ALLOCATOR ---help--- - Enable support for control of differential rovers + Enable support for differential rovers diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index c5fe294cd3..5ad4439597 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.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,12 +33,15 @@ #include "RoverDifferential.hpp" +using namespace time_literals; + RoverDifferential::RoverDifferential() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); updateParams(); - _rover_differential_setpoint_pub.advertise(); } bool RoverDifferential::init() @@ -50,219 +53,103 @@ bool RoverDifferential::init() void RoverDifferential::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } } void RoverDifferential::Run() { - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - updateSubscriptions(); - - // Generate and publish attitude, rate and speed 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_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (_max_yaw_rate > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { - const float scaled_yaw_rate_input = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - const float speed_diff = scaled_yaw_rate_input * _param_rd_wheel_track.get() / 2.f; - rover_differential_setpoint.speed_diff_setpoint_normalized = math::interpolate(speed_diff, - -_param_rd_max_thr_yaw_r.get(), _param_rd_max_thr_yaw_r.get(), -1.f, 1.f); - - } else { - rover_differential_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.roll; - - } - - rover_differential_setpoint.yaw_rate_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_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_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_STAB: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control - _yaw_ctl = false; - - - } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) - if (!_yaw_ctl) { - _stab_desired_yaw = _vehicle_yaw; - _yaw_ctl = true; - } - - rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - - } - - _rover_differential_setpoint_pub.publish(rover_differential_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_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); - rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control - _yaw_ctl = false; - - - } else { // Course control if the yaw rate input is zero (keep driving on a straight line) - if (!_yaw_ctl) { - _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); - _pos_ctl_start_position_ned = _curr_pos_ned; - _yaw_ctl = 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_differential_setpoint.forward_speed_setpoint) * - vector_scaling * _pos_ctl_course_direction; - // Calculate yaw setpoint - const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, - _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed)); - rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ? - yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards - rover_differential_setpoint.yaw_rate_setpoint = NAN; - - } - - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state); - break; - - default: // Unimplemented nav states will stop the rover - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - rover_differential_setpoint.speed_diff_setpoint_normalized = 0.f; - rover_differential_setpoint.yaw_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - break; - } - - if (!_armed) { // Reset when disarmed - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - } - - _rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed); - -} - -void RoverDifferential::updateSubscriptions() -{ - if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); updateParams(); } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); + 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 - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - } + _differential_pos_vel_control.updatePosVelControl(); + _differential_att_control.updateAttControl(); + _differential_rate_control.updateRateControl(); - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + 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]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f; + 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_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_control_mode.flag_armed) { + generateActuatorSetpoint(); + } - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - _vehicle_local_position_sub.copy(&vehicle_local_position); - _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; +} + +void RoverDifferential::generateSteeringSetpoint() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + 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_speed_diff = 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 RoverDifferential::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_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f; + } + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint, + _rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(throttle_body_x, + _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); +} + +Vector2f RoverDifferential::computeInverseKinematics(float throttle_body_x, const float speed_diff_normalized) +{ + float max_motor_command = fabsf(throttle_body_x) + fabsf(speed_diff_normalized); + + if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 + float excess = fabsf(max_motor_command - 1.0f); + throttle_body_x -= sign(throttle_body_x) * excess; + } + + // Calculate the left and right wheel speeds + return Vector2f(throttle_body_x - speed_diff_normalized, + throttle_body_x + speed_diff_normalized); +} + int RoverDifferential::task_spawn(int argc, char *argv[]) { RoverDifferential *instance = new RoverDifferential(); @@ -288,7 +175,7 @@ int RoverDifferential::task_spawn(int argc, char *argv[]) int RoverDifferential::custom_command(int argc, char *argv[]) { - return print_usage("unk_timestampn command"); + return print_usage("unknown command"); } int RoverDifferential::print_usage(const char *reason) @@ -300,7 +187,7 @@ int RoverDifferential::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Rover Differential controller. +Rover differential module. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("rover_differential", "controller"); diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 3fa8f4891c..9c943627b0 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.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,40 +39,34 @@ #include #include #include -#include + +// Libraries +#include +#include // uORB includes -#include #include -#include +#include +#include #include -#include -#include -#include -#include -#include - -// Standard libraries -#include +#include +#include +#include +#include +#include // Local includes -#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" -#include "RoverDifferentialControl/RoverDifferentialControl.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 YAW_RATE_THRESHOLD = - 0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero -static constexpr float SPEED_THRESHOLD = - 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero +#include "DifferentialRateControl/DifferentialRateControl.hpp" +#include "DifferentialAttControl/DifferentialAttControl.hpp" +#include "DifferentialPosVelControl/DifferentialPosVelControl.hpp" class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + /** + * @brief Constructor for RoverDifferential + */ RoverDifferential(); ~RoverDifferential() override = default; @@ -88,51 +82,66 @@ 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(); - // uORB Subscriptions - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + /** + * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void generateActuatorSetpoint(); + + /** + * @brief Compute normalized motor commands based on normalized setpoints. + * @param throttle_body_x Normalized speed in body x direction [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return Motor speeds for the right and left motors [-1, 1]. + */ + Vector2f computeInverseKinematics(float throttle_body_x, float speed_diff_normalized); + + // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + 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_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_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - // Instances - RoverDifferentialGuidance _rover_differential_guidance{this}; - RoverDifferentialControl _rover_differential_control{this}; - PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + // Class instances + DifferentialRateControl _differential_rate_control{this}; + DifferentialAttControl _differential_att_control{this}; + DifferentialPosVelControl _differential_pos_vel_control{this}; // Variables - Vector2f _curr_pos_ned{}; - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_yaw_rate{0.f}; - float _vehicle_forward_speed{0.f}; - float _vehicle_yaw{0.f}; - float _max_yaw_rate{0.f}; - int _nav_state{0}; - bool _armed{false}; - bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode - float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode - Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode - Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode + hrt_abstime _timestamp{0}; + float _dt{0.f}; + float _current_throttle_body_x{0.f}; + // Controllers + SlewRate _throttle_body_x_setpoint{0.f}; + + // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_max_thr_yaw_r, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_pp_lookahd_max + (ParamInt) _param_r_rev, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp deleted file mode 100644 index a378de3b94..0000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ /dev/null @@ -1,277 +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 "RoverDifferentialControl.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _rover_differential_status_pub.advertise(); -} - -void RoverDifferentialControl::updateParams() -{ - ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; - _max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F; - - // Update PID - _pid_yaw_rate.setGains(_param_rd_yaw_rate_p.get(), _param_rd_yaw_rate_i.get(), 0.f); - _pid_yaw_rate.setIntegralLimit(1.f); - _pid_yaw_rate.setOutputLimit(1.f); - _pid_throttle.setGains(_param_rd_p_gain_speed.get(), _param_rd_i_gain_speed.get(), 0.f); - _pid_throttle.setIntegralLimit(1.f); - _pid_throttle.setOutputLimit(1.f); - _pid_yaw.setGains(_param_rd_p_gain_yaw.get(), _param_rd_i_gain_yaw.get(), 0.f); - _pid_yaw.setIntegralLimit(_max_yaw_rate); - _pid_yaw.setOutputLimit(_max_yaw_rate); - - // Update slew rates - if (_max_yaw_rate > FLT_EPSILON) { - _yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate); - } - - if (_max_yaw_accel > FLT_EPSILON) { - _yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel); - } - -} - -void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, - const float vehicle_forward_speed) -{ - // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - // Update differential setpoint - _rover_differential_setpoint_sub.update(&_rover_differential_setpoint); - - // Closed loop yaw control (Overrides yaw rate setpoint) - if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { - _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt); - _rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); - _pid_yaw.setSetpoint( - matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - - vehicle_yaw)); // error as setpoint to take care of wrapping - _rover_differential_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); - _rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint; - - } else { - _yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw); - } - - // Yaw rate control - float speed_diff_normalized{0.f}; - - if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, - _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, false); - - } else { // Use normalized setpoint - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.speed_diff_setpoint_normalized, - vehicle_yaw_rate, - _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, true); - } - - // Speed control - float forward_speed_normalized{0.f}; - - if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { - if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { - - forward_speed_normalized = math::interpolate(_rover_differential_setpoint.forward_speed_setpoint, - -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(), -1.f, 1.f); - - if (fabsf(forward_speed_normalized) > 1.f - - fabsf(speed_diff_normalized)) { // Adjust forward speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels - _rover_differential_setpoint.forward_speed_setpoint = sign(_rover_differential_setpoint.forward_speed_setpoint) * - math::interpolate(1.f - fabsf(speed_diff_normalized), -1.f, 1.f, - -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get()); - } - } - - - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint, - vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, - _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, false); - - } else if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint_normalized, - vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, - _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, true); - - } - - // Publish rover differential status (logging) - _rover_differential_status.timestamp = _timestamp; - _rover_differential_status.measured_forward_speed = vehicle_forward_speed; - _rover_differential_status.measured_yaw = vehicle_yaw; - _rover_differential_status.measured_yaw_rate = vehicle_yaw_rate; - _rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - _rover_differential_status.pid_throttle_integral = _pid_throttle.getIntegral(); - _rover_differential_status.pid_yaw_integral = _pid_yaw.getIntegral(); - _rover_differential_status_pub.publish(_rover_differential_status); - - // Publish to motors - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); -} - -float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, - const float max_thr_yaw_r, - const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, - PID &pid_yaw_rate, 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 = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f; - } - - if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization); - yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt); - - } else { - yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint); - } - - // Transform yaw rate into speed difference - float speed_diff_normalized{0.f}; - - if (normalized) { - speed_diff_normalized = yaw_rate_with_accel_limit.getState(); - - } else { - _rover_differential_status.adjusted_yaw_rate_setpoint = yaw_rate_with_accel_limit.getState(); - - if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward - const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track / - 2.f; - speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, - max_thr_yaw_r, -1.f, 1.f); - } - - pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); - speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); - } - - return math::constrain(speed_diff_normalized, -1.f, 1.f); - -} - -float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint, - const float vehicle_forward_speed, const float max_thr_spd, SlewRate &forward_speed_setpoint_with_accel_limit, - PID &pid_throttle, const float max_accel, const float max_decel, 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 = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f; - } - - // Apply acceleration and deceleration limit - if (fabsf(forward_speed_setpoint) >= fabsf(forward_speed_setpoint_with_accel_limit.getState())) { - if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - forward_speed_setpoint_with_accel_limit.setSlewRate(max_accel / 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 (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - forward_speed_setpoint_with_accel_limit.setSlewRate(max_decel / 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_differential_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); - - if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - forward_speed_normalized = math::interpolate(_forward_speed_setpoint_with_accel_limit.getState(), - -max_thr_spd, max_thr_spd, - -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); - -} - -matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forward_speed_normalized, - const float speed_diff_normalized) -{ - float max_motor_command = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized); - - if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 - float excess = fabsf(max_motor_command - 1.0f); - forward_speed_normalized -= sign(forward_speed_normalized) * excess; - } - - // Calculate the left and right wheel speeds - return Vector2f(forward_speed_normalized - speed_diff_normalized, - forward_speed_normalized + speed_diff_normalized); -} - -void RoverDifferentialControl::resetControllers() -{ - _pid_throttle.resetIntegral(); - _pid_yaw_rate.resetIntegral(); - _pid_yaw.resetIntegral(); -} diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp deleted file mode 100644 index e08153356b..0000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ /dev/null @@ -1,169 +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 -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for differential rover guidance. - */ -class RoverDifferentialControl : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverDifferentialControl. - * @param parent The parent ModuleParams object. - */ - RoverDifferentialControl(ModuleParams *parent); - ~RoverDifferentialControl() = default; - - /** - * @brief Compute motor commands based on setpoints - */ - void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed); - - /** - * @brief Reset PID controllers - */ - void resetControllers(); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - - /** - * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates. - * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]]. - * @param vehicle_yaw_rate Measured yaw rate [rad/s]. - * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. - * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2]. - * @param wheel_track Wheel track [m]. - * @param dt Time since last update [s]. - * @param yaw_rate_with_accel_limit Yaw rate slew rate. - * @param pid_yaw_rate Yaw rate PID - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, - float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); - /** - * @brief Compute normalized forward speed setpoint and apply 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 max_thr_spd Speed the rover drives at maximum throttle [m/s]. - * @param forward_speed_setpoint_with_accel_limit Speed slew rate. - * @param pid_throttle Throttle PID - * @param max_accel Maximum acceleration [m/s^2] - * @param max_decel Maximum deceleration [m/s^2] - * @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 max_thr_spd, - SlewRate &forward_speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, - float dt, bool normalized); - - /** - * @brief Compute normalized motor commands based on normalized setpoints. - * @param forward_speed_normalized Normalized forward speed [-1, 1]. - * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. - * @return matrix::Vector2f Motor velocities for the right and left motors [-1, 1]. - */ - matrix::Vector2f computeInverseKinematics(float forward_speed_normalized, const float speed_diff_normalized); - - // uORB subscriptions - uORB::Subscription _rover_differential_setpoint_sub{ORB_ID(rover_differential_setpoint)}; - - // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; - rover_differential_status_s _rover_differential_status{}; - - // Variables - rover_differential_setpoint_s _rover_differential_setpoint{}; - hrt_abstime _timestamp{0}; - float _max_yaw_rate{0.f}; - float _max_yaw_accel{0.f}; - - // Controllers - PID _pid_throttle; // Closed loop speed control - PID _pid_yaw; // Closed loop yaw control - PID _pid_yaw_rate; // Closed loop yaw rate control - SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; - SlewRate _yaw_rate_with_accel_limit{0.f}; - SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_thr_spd, - (ParamFloat) _param_rd_max_accel, - (ParamFloat) _param_rd_max_decel, - (ParamFloat) _param_rd_max_thr_yaw_r, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_max_yaw_accel, - (ParamFloat) _param_rd_yaw_rate_p, - (ParamFloat) _param_rd_yaw_rate_i, - (ParamFloat) _param_rd_p_gain_speed, - (ParamFloat) _param_rd_i_gain_speed, - (ParamFloat) _param_rd_p_gain_yaw, - (ParamFloat) _param_rd_i_gain_yaw, - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp deleted file mode 100644 index bb45e6b7b9..0000000000 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ /dev/null @@ -1,234 +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 "RoverDifferentialGuidance.hpp" - -#include - -using namespace matrix; - -RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _max_forward_speed = _param_rd_max_speed.get(); - _rover_differential_guidance_status_pub.advertise(); -} - -void RoverDifferentialGuidance::updateParams() -{ - ModuleParams::updateParams(); -} - -void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const float forward_speed, const int nav_state) -{ - updateSubscriptions(); - - // Catch return to launch - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), _curr_wp(1)); - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - // State machine - float desired_yaw = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - math::max(forward_speed, 0.f)); - const float heading_error = matrix::wrap_pi(desired_yaw - vehicle_yaw); - - if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { - if (_currentState == GuidanceState::STOPPED) { - _currentState = GuidanceState::DRIVING; - } - - if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { - _currentState = GuidanceState::SPOT_TURNING; - - } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { - _currentState = GuidanceState::DRIVING; - } - - } else { // Mission finished or delay command - _currentState = GuidanceState::STOPPED; - } - - // Guidance logic - float desired_forward_speed{0.f}; - - switch (_currentState) { - case GuidanceState::DRIVING: { - // Calculate desired forward speed - desired_forward_speed = _max_forward_speed; - - if (_waypoint_transition_angle < M_PI_F - _param_rd_trans_drv_trn.get()) { - if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON) { - desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_decel.get(), distance_to_curr_wp, 0.0f); - desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed); - } - - } else if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON - && _param_rd_miss_spd_gain.get() > FLT_EPSILON) { - const float speed_reduction = math::constrain(_param_rd_miss_spd_gain.get() * math::interpolate( - M_PI_F - _waypoint_transition_angle, 0.f, - M_PI_F, 0.f, 1.f), 0.f, 1.f); - desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_decel.get(), distance_to_curr_wp, _max_forward_speed * (1.f - speed_reduction)); - desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, - _max_forward_speed); - } - - } break; - - case GuidanceState::SPOT_TURNING: - if (forward_speed > TURN_MAX_VELOCITY) { - desired_yaw = vehicle_yaw; // Wait for the rover to stop - - } - - break; - - case GuidanceState::STOPPED: - default: - desired_yaw = vehicle_yaw; - break; - - } - - // Publish differential guidance status (logging) - hrt_abstime timestamp = hrt_absolute_time(); - rover_differential_guidance_status_s rover_differential_guidance_status{}; - rover_differential_guidance_status.timestamp = timestamp; - rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; - rover_differential_guidance_status.state_machine = (uint8_t) _currentState; - _rover_differential_guidance_status_pub.publish(rover_differential_guidance_status); - - // Publish setpoints - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed; - rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = desired_yaw; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); -} - -void RoverDifferentialGuidance::updateSubscriptions() -{ - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position{}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } - - 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 (_position_setpoint_triplet_sub.updated()) { - updateWaypoints(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; - } - - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } -} - -void RoverDifferentialGuidance::updateWaypoints() -{ - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - - // Global waypoint coordinates - 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); - - } else { - _curr_wp = Vector2d(0, 0); - } - - 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); - - } else { - _prev_wp = _curr_pos; - } - - 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); - - } else { - _next_wp = _home_position; - } - - // NED waypoint coordinates - _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); - _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); - - // Distances - const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned; - const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned; - float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); - cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem - _waypoint_transition_angle = acosf(cosin); - - // Waypoint cruising speed - if (position_setpoint_triplet.current.cruising_speed > 0.f) { - _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); - - } else { - _max_forward_speed = _param_rd_max_speed.get(); - } -} diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp deleted file mode 100644 index cc1b033635..0000000000 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ /dev/null @@ -1,151 +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 - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Enum class for the different states of guidance. - */ -enum class GuidanceState { - SPOT_TURNING, // The vehicle is currently turning on the spot. - DRIVING, // The vehicle is currently driving. - STOPPED // The vehicle is stopped. -}; - -/** - * @brief Class for differential rover guidance. - */ -class RoverDifferentialGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverDifferentialGuidance. - * @param parent The parent ModuleParams object. - */ - RoverDifferentialGuidance(ModuleParams *parent); - ~RoverDifferentialGuidance() = default; - - /** - * @brief Compute and publish attitude and velocity setpoints based on the mission plan. - * @param vehicle_yaw The yaw of the vehicle [rad]. - * @param forward_speed The forward speed of the vehicle [m/s]. - * @param nav_state Navigation state of the rover. - */ - void computeGuidance(float vehicle_yaw, float forward_speed, int nav_state); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Update uORB subscriptions. - */ - void updateSubscriptions(); - - /** - * @brief Update global/ned waypoint coordinates - */ - void updateWaypoints(); - - // uORB subscriptions - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; - - // uORB publications - uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; - uORB::Publication _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; - - // Variables - MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. - GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. - PurePursuit _pure_pursuit{this}; // Pure pursuit library - bool _mission_finished{false}; - - // Waypoints - Vector2d _curr_pos{}; - Vector2f _curr_pos_ned{}; - Vector2d _prev_wp{}; - Vector2f _prev_wp_ned{}; - Vector2d _curr_wp{}; - Vector2f _curr_wp_ned{}; - Vector2d _next_wp{}; - Vector2f _next_wp_ned{}; - Vector2d _home_position{}; - float _max_forward_speed{0.f}; // Maximum forward speed for the rover [m/s] - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - - // Constants - static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s] - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rd_max_jerk, - (ParamFloat) _param_rd_max_decel, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_rd_trans_trn_drv, - (ParamFloat) _param_rd_trans_drv_trn, - (ParamFloat) _param_rd_miss_spd_gain - ) -}; diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 6297f3aafd..eca54e798e 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -3,176 +3,17 @@ module_name: Rover Differential parameters: - group: Rover Differential definitions: - RD_WHEEL_TRACK: - description: - short: Wheel track - long: Distance from the center of the right wheel to the center of the left wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.5 - - RD_YAW_P: - description: - short: Proportional gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_YAW_I: - description: - short: Integral gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_SPEED_P: - description: - short: Proportional gain for closed loop forward speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_SPEED_I: - description: - short: Integral gain for closed loop forward speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_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: 1 - - RD_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 - - RD_MAX_JERK: - description: - short: Maximum jerk - long: Limit for forwards acc/deceleration change. - type: float - unit: m/s^3 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RD_MAX_ACCEL: - description: - short: Maximum acceleration - long: Maximum acceleration is used to limit the acceleration of the rover - type: float - unit: m/s^2 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RD_MAX_DECEL: - description: - short: Maximum deceleration - long: | - Maximum decelaration is used to limit the deceleration of the rover. - Set to -1 to disable, causing the rover to decelerate as fast as possible. - Caution: This disables the slow down effect in auto modes. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RD_MAX_SPEED: - description: - short: Maximum speed setpoint - long: | - This parameter is used to cap desired forward speed and map controller inputs to desired speeds in Position mode. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RD_MAX_THR_SPD: - 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: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RD_MAX_YAW_RATE: - description: - short: Maximum allowed yaw rate for the rover - long: | - This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates - in Acro,Stabilized and Position mode. - type: float - unit: deg/s - min: 0.01 - max: 1000 - increment: 0.01 - decimal: 2 - default: 90 - - RD_MAX_YAW_ACCEL: - description: - short: Maximum allowed yaw acceleration for the rover - long: | - This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints - to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change. - This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track. - Set to -1 to disable. - type: float - unit: deg/s^2 - min: -1 - max: 1000 - increment: 0.01 - decimal: 2 - default: -1 + description: + short: Wheel track + long: Distance from the center of the right wheel to the center of the left wheel. + type: float + unit: m + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 RD_MAX_THR_YAW_R: description: @@ -181,7 +22,7 @@ parameters: This parameter is used to calculate the feedforward term of the closed loop yaw rate control. The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate. This desired speed difference is then linearly mapped to normalized motor commands. - A good starting point is twice the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). + A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower. type: float unit: m/s @@ -189,11 +30,14 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 2 + default: 0 RD_TRANS_TRN_DRV: description: short: Yaw error threshhold to switch from spot turning to driving + long: | + This threshold is used for the state machine to switch from turning to driving based on the + error between the desired and actual yaw. type: float unit: rad min: 0.001 @@ -227,9 +71,10 @@ parameters: The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP interpolated from [0, 180] -> [0, 1]. Higher value -> More speed reduction during waypoint transitions. + Set to -1 to disable any speed reduction during waypoint transition. type: float - min: 0.05 + min: -1 max: 100 increment: 0.01 decimal: 2 - default: 1 + default: -1