From 14f128b89dc1e82e71881bdefcdf145d64a5f540 Mon Sep 17 00:00:00 2001 From: baumanta Date: Mon, 29 Jul 2019 17:51:08 +0200 Subject: [PATCH] add parameter for detection angle --- .../CollisionPrevention.cpp | 25 ++++++++++--------- .../CollisionPrevention.hpp | 1 + .../collisionprevention_params.c | 12 +++++++++ 3 files changed, 26 insertions(+), 12 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index cbdde6f623..2773239f11 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -212,21 +212,22 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, angle += math::radians(obstacle.angle_offset); } + //check if the bin must be considered regarding the given stick input Vector2f bin_direction = {cos(angle), sin(angle)}; - float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction)); - //calculate max allowed velocity with a P-controller (same gain as in the position controller) - float delay_distance = curr_vel_parallel * _param_mpc_col_prev_dly.get(); - float vel_max_posctrl = math::max(0.f, - _param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance)); - Vector2f vel_max_vec = bin_direction * vel_max_posctrl; - float vel_max_bin = vel_max_vec.dot(setpoint_dir); - float vel_setpoint_bin = setpoint.dot(bin_direction); + if (setpoint_dir.dot(bin_direction) > 0 && setpoint_dir.dot(bin_direction) > cosf(_param_mpc_col_prev_ang.get())) { + //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 * _param_mpc_col_prev_dly.get(); + float vel_max_posctrl = math::max(0.f, + _param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance)); + Vector2f vel_max_vec = bin_direction * vel_max_posctrl; + float vel_max_bin = vel_max_vec.dot(setpoint_dir); - //limit the velocity - //do not react to obstacles more than 45 degree off the given stick input cos(45deg) = 0.71 - if (vel_setpoint_bin > 0.71f * setpoint_length && vel_max_bin >= 0) { - vel_max = math::min(vel_max, vel_max_bin); + //constrain the velocity + if (vel_max_bin >= 0) { + vel_max = math::min(vel_max, vel_max_bin); + } } } } diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index bd17c45020..5433a1fad1 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -94,6 +94,7 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */ + (ParamFloat) _param_mpc_col_prev_ang, /**< collision prevention detection angle */ (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ (ParamFloat) _param_mpc_col_prev_dly /**< delay of the range measurement data*/ ) diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index a69a9ccc95..5f7c97a497 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -62,3 +62,15 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.f); + +/** + * Angle at which the vehicle can fly away from obstacles + * + * Only used in Position mode. + * + * @min 0 + * @max 1.570796 + * @unit [ ] + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_COL_PREV_ANG, 0.785f);