dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add collision prevention velocity limitations also based on max accel/jerk

This commit is contained in:
Julian Kent 2019-08-28 13:58:21 +02:00
parent 60e5e0821a
commit 865157228f
2 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/lib/CollisionPrevention/CollisionPrevention.cpp
View File

@ -38,6 +38,9 @@
*/
#include <CollisionPrevention/CollisionPrevention.hpp>
#include <FlightTasks/tasks/Utility/TrajMath.hpp>
using namespace matrix;
using namespace time_literals;
@ -209,6 +212,8 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
float col_prev_dly = _param_mpc_col_prev_dly.get();
float col_prev_ang_rad = math::radians(_param_mpc_col_prev_ang.get());
float xy_p = _param_mpc_xy_p.get();
float max_jerk = _param_mpc_jerk_max.get();
float max_accel = _param_mpc_acc_hor.get();
if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) {
if (setpoint_length > 0.001f) {
@ -240,8 +245,10 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
//calculate max allowed velocity with a P-controller (same gain as in the position controller)
float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
float delay_distance = curr_vel_parallel * col_prev_dly;
float vel_max_posctrl = math::max(0.f, xy_p * (distance - min_dist_to_keep - delay_distance));
Vector2f vel_max_vec = bin_direction * vel_max_posctrl;
float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
float vel_max_posctrl = xy_p * stop_distance;
float vel_max_smooth = trajmath::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);
Vector2f vel_max_vec = bin_direction * math::min(vel_max_posctrl, vel_max_smooth);
float vel_max_bin = vel_max_vec.dot(setpoint_dir);
//constrain the velocity

4
src/lib/CollisionPrevention/CollisionPrevention.hpp
View File

@ -96,7 +96,9 @@ private:
(ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */
(ParamFloat<px4::params::MPC_COL_PREV_ANG>) _param_mpc_col_prev_ang, /**< collision prevention detection angle */
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
(ParamFloat<px4::params::MPC_COL_PREV_DLY>) _param_mpc_col_prev_dly /**< delay of the range measurement data*/
(ParamFloat<px4::params::MPC_COL_PREV_DLY>) _param_mpc_col_prev_dly, /**< delay of the range measurement data*/
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor
)
/**