From 62afec1634dfaeb8f636492cab8c924698daa30b Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 18 Nov 2024 16:27:28 +0100 Subject: [PATCH] differential: add slow down effect in mission mode --- .../init.d-posix/airframes/4009_gz_r1_rover | 1 - .../init.d-posix/airframes/4011_gz_lawnmower | 1 - .../airframes/50001_aion_robotics_r1_rover | 1 - .../RoverDifferentialGuidance.cpp | 14 ++++++++-- .../RoverDifferentialGuidance.hpp | 5 ++-- src/modules/rover_differential/module.yaml | 27 +++++++++++-------- 6 files changed, 30 insertions(+), 19 deletions(-) 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 ffbdfad2f9be..3f96dad5f883 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 @@ -26,7 +26,6 @@ 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_MISS_SPD_DEF 2 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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 5f2b635238f7..d4c80a29092f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -29,7 +29,6 @@ param set-default RD_MAX_SPEED 8 param set-default RD_YAW_P 5 param set-default RD_YAW_I 0.1 param set-default RD_MAX_YAW_RATE 30 -param set-default RD_MISS_SPD_DEF 8 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 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 859d589d6ac5..7cdf2d10a673 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 @@ -31,7 +31,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_MISS_SPD_DEF 1.5 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 diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index 400cd643f3d7..bb45e6b7b99c 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -40,7 +40,7 @@ using namespace matrix; RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _max_forward_speed = _param_rd_miss_spd_def.get(); + _max_forward_speed = _param_rd_max_speed.get(); _rover_differential_guidance_status_pub.advertise(); } @@ -96,6 +96,16 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f _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; @@ -219,6 +229,6 @@ void RoverDifferentialGuidance::updateWaypoints() _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); } else { - _max_forward_speed = _param_rd_miss_spd_def.get(); + _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 index c5d3a3f1734f..cc1b03363537 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -144,9 +144,8 @@ class RoverDifferentialGuidance : public ModuleParams (ParamFloat) _param_rd_max_jerk, (ParamFloat) _param_rd_max_decel, (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_rd_miss_spd_def, (ParamFloat) _param_rd_trans_trn_drv, - (ParamFloat) _param_rd_trans_drv_trn - + (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 d1de0cbddb9f..6297f3aafd78 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -191,17 +191,6 @@ parameters: decimal: 2 default: 2 - RD_MISS_SPD_DEF: - description: - short: Default forward speed for the rover during auto modes - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - RD_TRANS_TRN_DRV: description: short: Yaw error threshhold to switch from spot turning to driving @@ -228,3 +217,19 @@ parameters: increment: 0.01 decimal: 3 default: 0.174533 + + RD_MISS_SPD_GAIN: + description: + short: Tuning parameter for the speed reduction during waypoint transition + long: | + The waypoint transition speed is calculated as: + Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN) + 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. + type: float + min: 0.05 + max: 100 + increment: 0.01 + decimal: 2 + default: 1