ColisionAvoidance: move instantiation to FlightTask

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
  *