use only one parameter and enable parameter change in flight. clean up code

src/lib/CollisionPrevention/CollisionPrevention.cpp

@@ -38,10 +38,12 @@
  */
 
 #include <CollisionPrevention/CollisionPrevention.hpp>
+using namespace matrix;
+using namespace time_literals;
 
 
-CollisionPrevention::CollisionPrevention() :
-	ModuleParams(nullptr)
+CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
+	ModuleParams(parent)
 {
 
 }
@@ -105,17 +107,20 @@ void CollisionPrevention::publish_constraints(const Vector2f &original_setpoint,
 
 void CollisionPrevention::update_range_constraints()
 {
-	obstacle_distance_s obstacle_distance = _sub_obstacle_distance->get();
+	const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance->get();
 
 	if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) {
 		float max_detection_distance = obstacle_distance.max_distance / 100.0f; //convert to meters
 
-		for (int i = 0; i < 72; i++) {
+		int distances_array_size = sizeof(obstacle_distance.distances) / sizeof(obstacle_distance.distances[0]);
+
+		for (int i = 0; i < distances_array_size; i++) {
 			//determine if distance bin is valid and contains a valid distance measurement
 			if (obstacle_distance.distances[i] < obstacle_distance.max_distance &&
 			    obstacle_distance.distances[i] > obstacle_distance.min_distance && i * obstacle_distance.increment < 360) {
 				float distance = obstacle_distance.distances[i] / 100.0f; //convert to meters
-				float angle = i * obstacle_distance.increment * (M_PI / 180.0);
+				float angle = math::radians((float)i * obstacle_distance.increment);
+
 				//calculate normalized velocity reductions
 				float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * cos(angle);
 				float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * sin(angle);
@@ -146,7 +151,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
 	_move_constraints_y = _move_constraints_y_normalized;
 
 	// calculate the maximum velocity along x,y axis when moving in the demanded direction
-	float vel_mag = sqrt(original_setpoint(0) * original_setpoint(0) + original_setpoint(1) * original_setpoint(1));
+	float vel_mag = original_setpoint.norm();
 	float v_max_x, v_max_y;
 
 	if (vel_mag > 0.0f) {
@@ -169,7 +174,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
 	_move_constraints_y(1) = v_max_y - _move_constraints_y(1);
 
 	//constrain the velocity setpoint to respect the velocity limits
-	Vector2f new_setpoint = original_setpoint;
+	Vector2f new_setpoint;
 	new_setpoint(0) = math::constrain(original_setpoint(0), -_move_constraints_x(0), _move_constraints_x(1));
 	new_setpoint(1) = math::constrain(original_setpoint(1), -_move_constraints_y(0), _move_constraints_y(1));
 

src/lib/CollisionPrevention/CollisionPrevention.hpp

@@ -55,13 +55,10 @@ using uORB::Publication;
 #include <systemlib/mavlink_log.h>
 #include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
 
-using namespace matrix;
-using namespace time_literals;
-
 class CollisionPrevention : public ModuleParams
 {
 public:
-	CollisionPrevention();
+	CollisionPrevention(ModuleParams *parent);
 
 	~CollisionPrevention();
 
@@ -71,17 +68,9 @@ public:
 	 */
 	bool initializeSubscriptions(SubscriptionArray &subscription_array);
 
-	bool is_active() {return MPC_COL_PREV.get();}
-
-	void update();
-
-	void update_range_constraints();
-
-	void reset_constraints();
-
-	void publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint);
+	bool is_active() {return MPC_COL_PREV_D.get() > 0 ;}
 
-	void modifySetpoint(Vector2f &original_setpoint, const float max_speed);
+	void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed);
 
 private:
 
@@ -94,18 +83,25 @@ private:
 	uORB::Subscription<obstacle_distance_s> *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */
 
 	static constexpr uint64_t RANGE_STREAM_TIMEOUT_US = 500000;
-	static constexpr uint64_t MESSAGE_THROTTLE_US = 5_s;
+	static constexpr uint64_t MESSAGE_THROTTLE_US = 5000000;
 
 	hrt_abstime _last_message;
 
-	Vector2f _move_constraints_x_normalized;
-	Vector2f _move_constraints_y_normalized;
-	Vector2f _move_constraints_x;
-	Vector2f _move_constraints_y;
+	matrix::Vector2f _move_constraints_x_normalized;
+	matrix::Vector2f _move_constraints_y_normalized;
+	matrix::Vector2f _move_constraints_x;
+	matrix::Vector2f _move_constraints_y;
 
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::MPC_COL_PREV>) MPC_COL_PREV, /**< use range sensor measurements to prevent collision */
 		(ParamFloat<px4::params::MPC_COL_PREV_D>) MPC_COL_PREV_D /**< collision prevention keep minimum distance */
 	)
 
+	void update();
+
+	void update_range_constraints();
+
+	void reset_constraints();
+
+	void publish_constraints(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
+
 };

src/lib/CollisionPrevention/collisionprevention_params.c

@@ -39,22 +39,14 @@
  * @author Tanja Baumann <tanja@auterion.com>
  */
 
-/**
- * Flag to enable the use of a MAVLink range sensor for collision avoidance.
- *
- * @boolean
- * @group Multicopter Position Control
- */
-PARAM_DEFINE_INT32(MPC_COL_PREV, 0);
-
 /**
  * Minimum distance the vehicle should keep to all obstacles
  *
- * Only used in Position mode when collision avoidance is active (see MPC_COL_AVOID).
+ * Only used in Position mode. Collision avoidace is disable by setting this parameter to a negative value
  *
- * @min 0
+ * @min -1
  * @max 15
  * @unit meters
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, 4.0f);
+PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f);

src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -41,6 +41,11 @@
 
 using namespace matrix;
 
+FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this)
+{
+
+}
+
 bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscription_array)
 {
 	if (!FlightTaskManualAltitude::initializeSubscriptions(subscription_array)) {

src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -46,7 +46,7 @@
 class FlightTaskManualPosition : public FlightTaskManualAltitude
 {
 public:
-	FlightTaskManualPosition() = default;
+	FlightTaskManualPosition();
 
 	virtual ~FlightTaskManualPosition() = default;
 	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;