fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

CollisionPrevention: Added Case where velocity gets reduced to zero if we are closer to the obstacle than the minimal distance

Browse Source
This commit is contained in:
Claudio Chies 2024-11-20 16:39:18 +01:00
parent bbc59dcde7
commit db13b9cb50
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/lib/collision_prevention/CollisionPrevention.cpp
View File

@ -600,12 +600,18 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic
delay_distance += curr_vel_parallel * (data_age * 1e-6f);
}
const float stop_distance = math::max(0.f, distance - _min_dist_to_keep - delay_distance);
const float stop_distance = distance - _min_dist_to_keep - delay_distance;
const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(),
_param_mpc_acc_hor.get(), stop_distance, 0.f);
// we dont take the minimum of the last term because of stop_distance is zero but current velocity is not, we want the acceleration to become negative and slow us down.
const float curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * (max_vel - curr_vel_parallel);
float curr_acc_vel_constraint;
if (stop_distance >= 0.f) {
const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(),
_param_mpc_acc_hor.get(), stop_distance, 0.f);
curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * math::min(max_vel - curr_vel_parallel, 0.f);
} else {
curr_acc_vel_constraint = -1.f * _param_mpc_xy_vel_p_acc.get() * curr_vel_parallel;
}
if (curr_acc_vel_constraint < vel_comp_accel) {
vel_comp_accel = curr_acc_vel_constraint;