add parameter for detection angle

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);
-
-					//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);
+
+					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);
+
+						//constrain the velocity
+						if (vel_max_bin >= 0) {
+							vel_max = math::min(vel_max, vel_max_bin);
+						}
 					}
 				}
 			}

src/lib/CollisionPrevention/CollisionPrevention.hpp

@@ -94,6 +94,7 @@ class CollisionPrevention : public ModuleParams
 
 	DEFINE_PARAMETERS(
 		(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*/
 	)

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);