diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 2773239f11..c64be5bc30 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -215,7 +215,8 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, //check if the bin must be considered regarding the given stick input Vector2f bin_direction = {cos(angle), sin(angle)}; - if (setpoint_dir.dot(bin_direction) > 0 && setpoint_dir.dot(bin_direction) > cosf(_param_mpc_col_prev_ang.get())) { + if (setpoint_dir.dot(bin_direction) > 0 + && setpoint_dir.dot(bin_direction) > cosf(math::radians(_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(); diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index ca7f77fbf9..664e4c05cc 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -64,13 +64,13 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f); PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.f); /** - * Angle at which the vehicle can fly away from obstacles + * Angle left/right from the commanded setpoint in which the range data is used to calculate speed limitations. All data further from the commanded direction is not considered. * * Only used in Position mode. * * @min 0 - * @max 1.570796 - * @unit [rad] + * @max 90 + * @unit [deg] * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_COL_PREV_ANG, 0.785f); +PARAM_DEFINE_FLOAT(MPC_COL_PREV_ANG, 45.f);