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/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp index deb0b9e37b82..b9ad24799156 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp @@ -58,19 +58,19 @@ void RoverDifferentialControl::updateParams() _param_rd_yaw_rate_p.get(), // Proportional gain _param_rd_yaw_rate_i.get(), // Integral gain 0.f, // Derivative gain - 1.f, // Integral limit + _param_rd_yaw_rate_i.get() > FLT_EPSILON ? 1.f / _param_rd_yaw_rate_i.get() : 0.f, // Integral limit 1.f); // Output limit pid_set_parameters(&_pid_throttle, - _param_rd_p_gain_speed.get(), // Proportional gain - _param_rd_i_gain_speed.get(), // Integral gain + _param_rd_speed_p.get(), // Proportional gain + _param_rd_speed_i.get(), // Integral gain 0.f, // Derivative gain - 1.f, // Integral limit + _param_rd_speed_i.get() > FLT_EPSILON ? 1.f / _param_rd_speed_i.get() : 0.f, // Integral limit 1.f); // Output limit pid_set_parameters(&_pid_yaw, - _param_rd_p_gain_yaw.get(), // Proportional gain - _param_rd_i_gain_yaw.get(), // Integral gain + _param_rd_yaw_p.get(), // Proportional gain + _param_rd_yaw_i.get(), // Integral gain 0.f, // Derivative gain - _max_yaw_rate, // Integral limit + _param_rd_yaw_i.get() > FLT_EPSILON ? _max_yaw_rate / _param_rd_yaw_i.get() : 0.f, // Integral limit _max_yaw_rate); // Output limit // Update slew rates @@ -126,6 +126,20 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con 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); @@ -142,9 +156,9 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con _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.integral; - _rover_differential_status.pid_throttle_integral = _pid_throttle.integral; - _rover_differential_status.pid_yaw_integral = _pid_yaw.integral; + _rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral * _param_rd_yaw_rate_i.get(); + _rover_differential_status.pid_throttle_integral = _pid_throttle.integral * _param_rd_speed_i.get(); + _rover_differential_status.pid_yaw_integral = _pid_yaw.integral * _param_rd_yaw_i.get(); _rover_differential_status_pub.publish(_rover_differential_status); // Publish to motors diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp index 5a445e788234..7d5adccb0e1b 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp @@ -160,10 +160,10 @@ class RoverDifferentialControl : public ModuleParams (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, + (ParamFloat) _param_rd_speed_p, + (ParamFloat) _param_rd_speed_i, + (ParamFloat) _param_rd_yaw_p, + (ParamFloat) _param_rd_yaw_i, (ParamInt) _param_r_rev ) }; 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