ColisionAvoidance: move instantiation to FlightTask

src/lib/CollisionAvoidance/CollisionAvoidance.cpp

@@ -46,6 +46,15 @@ CollisionAvoidance::CollisionAvoidance() :
 
 }
 
+bool CollisionAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array)
+{
+	if (!subscription_array.get(ORB_ID(obstacle_distance), _sub_obstacle_distance)) {
+		return false;
+	}
+
+	return true;
+}
+
 void CollisionAvoidance::reset_constraints()
 {
 
@@ -88,7 +97,8 @@ void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint,
 
 }
 
-void CollisionAvoidance::update(const obstacle_distance_s &distance_measurements) {
+void CollisionAvoidance::update()
+{
 	// activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter
 	if (collision_avoidance_enabled()) {
 		activate();
@@ -96,21 +106,21 @@ void CollisionAvoidance::update(const obstacle_distance_s &distance_measurements
 	} else {
 		deactivate();
 	}
-
-	_obstacle_distance = distance_measurements;
 }
 
 void CollisionAvoidance::update_range_constraints()
 {
-	if (hrt_elapsed_time((hrt_abstime *)&_obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) {
-		float max_detection_distance = _obstacle_distance.max_distance / 100.0f; //convert to meters
+	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++) {
 			//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);
+			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);
 				//calculate normalized velocity reductions
 				float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_AVOID_D.get()) * cos(angle);
 				float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_AVOID_D.get()) * sin(angle);
@@ -133,7 +143,7 @@ void CollisionAvoidance::update_range_constraints()
 
 void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float max_speed)
 {
-
+	update();
 	reset_constraints();
 
 	//calculate movement constraints based on range data

src/lib/CollisionAvoidance/CollisionAvoidance.hpp

@@ -53,6 +53,7 @@
 using uORB::Publication;
 #include <uORB/uORB.h>
 #include <systemlib/mavlink_log.h>
+#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
 
 using namespace matrix;
 using namespace time_literals;
@@ -64,6 +65,12 @@ public:
 
 	~CollisionAvoidance() = default;
 
+	/**
+	 * Initialize the uORB subscriptions using an array
+	 * @return true on success, false on error
+	 */
+	bool initializeSubscriptions(SubscriptionArray &subscription_array);
+
 	void activate() {_is_active = true;}
 
 	void deactivate() {_is_active = false;}
@@ -72,7 +79,7 @@ public:
 
 	bool collision_avoidance_enabled() { return MPC_COL_AVOID.get(); }
 
-	void update(const obstacle_distance_s &distance_measurements);
+	void update();
 
 	void update_range_constraints();
 
@@ -84,14 +91,14 @@ public:
 
 private:
 
-	obstacle_distance_s			_obstacle_distance{};	/**< obstacle distances received form a range sensor */
-
 	bool _is_active = true;
 	bool _interfering = false;		/**< states if the collision avoidance interferes with the user input */
 
 	orb_advert_t _constraints_pub{nullptr};		/**< constraints publication */
 	orb_advert_t _mavlink_log_pub = nullptr;	 	/**< Mavlink log uORB handle */
 
+	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;
 

src/lib/FlightTasks/FlightTasks.hpp

@@ -45,7 +45,6 @@
 #include "SubscriptionArray.hpp"
 #include "FlightTasks_generated.hpp"
 #include <lib/WeatherVane/WeatherVane.hpp>
-#include <lib/CollisionAvoidance/CollisionAvoidance.hpp>
 
 #include <new>
 
@@ -132,11 +131,6 @@ public:
 	 */
 	void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);}
 
-	/**
-	 * Sets an external collision avoidance. The active flight task can use the the collision avoidance to modify the setpoint.
-	 */
-	void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) {_current_task.task->setCollisionAvoidance(ext_collision_avoidance);}
-
 	/**
 	 *   This method will re-activate current task.
 	 */

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -52,7 +52,6 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include <lib/WeatherVane/WeatherVane.hpp>
-#include <lib/CollisionAvoidance/CollisionAvoidance.hpp>
 #include "SubscriptionArray.hpp"
 
 class FlightTask : public ModuleParams
@@ -155,12 +154,6 @@ public:
 
 	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp,
 					const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; }
-	
-	/**
-	 * Sets an external collision avoidance which can be used by any flight task to implement a different setpoint
-	 * This method does nothing, each flighttask which wants to use the collision avoidance needs to override this method.
-	 */
-	virtual void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) {};
 
 protected:
 

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

@@ -41,6 +41,19 @@
 
 using namespace matrix;
 
+bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscription_array)
+{
+	if (!FlightTaskManualAltitude::initializeSubscriptions(subscription_array)) {
+		return false;
+	}
+
+	if (!_collision_avoidance.initializeSubscriptions(subscription_array)) {
+		return false;
+	}
+
+	return true;
+}
+
 bool FlightTaskManualPosition::updateInitialize()
 {
 	bool ret = FlightTaskManualAltitude::updateInitialize();
@@ -114,9 +127,9 @@ void FlightTaskManualPosition::_scaleSticks()
 	/* Rotate setpoint into local frame. */
 	_rotateIntoHeadingFrame(vel_sp_xy);
 
-	//collision avoidance
-	if (_ext_collision_avoidance != nullptr && _ext_collision_avoidance->is_active()) {
-		_ext_collision_avoidance->modifySetpoint(vel_sp_xy, _constraints.speed_xy);
+	// collision avoidance
+	if (_collision_avoidance.is_active()) {
+		_collision_avoidance.modifySetpoint(vel_sp_xy, _constraints.speed_xy);
 	}
 
 	_velocity_setpoint(0) = vel_sp_xy(0);

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

@@ -41,6 +41,7 @@
 #pragma once
 
 #include "FlightTaskManualAltitude.hpp"
+#include <lib/CollisionAvoidance/CollisionAvoidance.hpp>
 
 class FlightTaskManualPosition : public FlightTaskManualAltitude
 {
@@ -48,6 +49,7 @@ public:
 	FlightTaskManualPosition() = default;
 
 	virtual ~FlightTaskManualPosition() = default;
+	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 	bool activate() override;
 	bool updateInitialize() override;
 
@@ -56,11 +58,6 @@ public:
 	 */
 	void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
 
-	/**
-	 * Sets an external collision avoidance which can be used to modify setpoints
-	 */
-	void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) override {_ext_collision_avoidance = ext_collision_avoidance;}
-
 
 protected:
 	void _updateXYlock(); /**< applies position lock based on stick and velocity */
@@ -80,5 +77,5 @@ private:
 	WeatherVane *_weathervane_yaw_handler =
 		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 
-	CollisionAvoidance *_ext_collision_avoidance = nullptr;	/**< external collision avoidance library*/
+	CollisionAvoidance _collision_avoidance;	/**< collision avoidance setpoint amendment */
 };

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -47,7 +47,6 @@
 #include <commander/px4_custom_mode.h>
 
 #include <uORB/topics/home_position.h>
-#include <uORB/topics/obstacle_distance.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -65,7 +64,6 @@
 
 #include <lib/FlightTasks/FlightTasks.hpp>
 #include <lib/WeatherVane/WeatherVane.hpp>
-#include <lib/CollisionAvoidance/CollisionAvoidance.hpp>
 #include "PositionControl.hpp"
 #include "Utility/ControlMath.hpp"
 
@@ -119,7 +117,6 @@ private:
 	int		_att_sub{-1};				/**< vehicle attitude */
 	int		_home_pos_sub{-1}; 			/**< home position */
 	int		_traj_wp_avoidance_sub{-1};	/**< trajectory waypoint */
-	int		_range_sensor_sub{-1};	    /**< obstacle distances */
 
 	int _task_failure_count{0};         /**< counter for task failures */
 
@@ -136,7 +133,6 @@ private:
 	home_position_s				_home_pos{};			/**< home position */
 	vehicle_trajectory_waypoint_s		_traj_wp_avoidance{};		/**< trajectory waypoint */
 	vehicle_trajectory_waypoint_s		_traj_wp_avoidance_desired{};	/**< desired waypoints, inputs to an obstacle avoidance module */
-	obstacle_distance_s			_obstacle_distance{};	/**< obstacle distances received form a range sensor */
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */
@@ -185,7 +181,6 @@ private:
 	systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */
 
 	WeatherVane *_wv_controller{nullptr};
-	CollisionAvoidance *_ca_controller{nullptr};
 
 	/**
 	 * Update our local parameter cache.
@@ -360,9 +355,6 @@ MulticopterPositionControl::~MulticopterPositionControl()
 	if (_wv_controller != nullptr) {
 		delete _wv_controller;
 	}
-	if (_ca_controller != nullptr) {
-		delete _ca_controller;
-	}
 }
 
 void
@@ -433,10 +425,6 @@ MulticopterPositionControl::poll_subscriptions()
 		if (_wv_controller == nullptr && _vehicle_status.is_vtol) {
 			_wv_controller = new WeatherVane();
 		}
-		// if the vehicle is a rotary wing, enable collision avoidance capabilities
-		if (_ca_controller == nullptr && _vehicle_status.is_rotary_wing) {
-			_ca_controller = new CollisionAvoidance();
-		}
 	}
 
 	orb_check(_vehicle_land_detected_sub, &updated);
@@ -477,12 +465,6 @@ MulticopterPositionControl::poll_subscriptions()
 	if (updated) {
 		orb_copy(ORB_ID(vehicle_trajectory_waypoint), _traj_wp_avoidance_sub, &_traj_wp_avoidance);
 	}
-
-	orb_check(_range_sensor_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(obstacle_distance), _range_sensor_sub, &_obstacle_distance);
-	}
 }
 
 void
@@ -600,7 +582,6 @@ MulticopterPositionControl::run()
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
 	_traj_wp_avoidance_sub = orb_subscribe(ORB_ID(vehicle_trajectory_waypoint));
-	_range_sensor_sub = orb_subscribe(ORB_ID(obstacle_distance));
 
 	parameters_update(true);
 
@@ -663,10 +644,6 @@ MulticopterPositionControl::run()
 			_wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw);
 		}
 
-		if (_ca_controller != nullptr) {
-			_ca_controller->update(_obstacle_distance);
-		}
-
 		if (_control_mode.flag_armed) {
 			// as soon vehicle is armed check for flighttask
 			start_flight_task();
@@ -688,7 +665,6 @@ MulticopterPositionControl::run()
 			vehicle_local_position_setpoint_s setpoint;
 
 			_flight_tasks.setYawHandler(_wv_controller);
-			_flight_tasks.setCollisionAvoidance(_ca_controller);
 
 			// update task
 			if (!_flight_tasks.update()) {

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -745,6 +745,7 @@ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_INT32(MPC_COL_AVOID, 0);
+
 /**
  * Minimum distance the vehicle should keep to all obstacles
  *