Add a collision avoidance library which uses range data

msg/CMakeLists.txt

@@ -42,6 +42,7 @@ set(msg_files
 	camera_capture.msg
 	camera_trigger.msg
 	collision_report.msg
+	collision_constraints.msg
 	commander_state.msg
 	cpuload.msg
 	debug_array.msg

msg/collision_constraints.msg

@@ -0,0 +1,12 @@
+# Local setpoint constraints in NED frame
+# setting something to NaN means that no limit is provided
+
+uint64 timestamp	# time since system start (microseconds)
+
+#value of 0 means no constraint along this axis, value of 1 means no movements alowed, value bigger than 1 forces negative movement.
+float32[2] constraints_normalized_x   # constraints determined by range sensor measurements [x negative, x positive]
+float32[2] constraints_normalized_y   # constraintss determined by range sensor measurements [y negative, y positive]
+
+float32[2] original_setpoint   # velocities demanded
+float32[2] adapted_setpoint    # velocities allowed
+

src/lib/CMakeLists.txt

@@ -56,3 +56,4 @@ add_subdirectory(terrain_estimation)
 add_subdirectory(tunes)
 add_subdirectory(version)
 add_subdirectory(WeatherVane)
+add_subdirectory(CollisionAvoidance)

src/lib/CollisionAvoidance/CMakeLists.txt

@@ -0,0 +1,34 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(CollisionAvoidance CollisionAvoidance.cpp)

src/lib/CollisionAvoidance/CollisionAvoidance.cpp

@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file CollisionAvoidance.cpp
+ * CollisionAvoidance controller.
+ *
+ */
+
+#include "CollisionAvoidance.hpp"
+
+
+CollisionAvoidance::CollisionAvoidance() :
+	ModuleParams(nullptr)
+{
+
+}
+
+void CollisionAvoidance::reset_constraints()
+{
+
+	_move_constraints_x_normalized(0) = 0.0f;  //normalized constraint in negative x-direction
+	_move_constraints_x_normalized(1) = 0.0f;  //normalized constraint in positive x-direction
+	_move_constraints_y_normalized(0) = 0.0f;  //normalized constraint in negative y-direction
+	_move_constraints_y_normalized(1) = 0.0f;  //normalized constraint in positive y-direction
+
+	_move_constraints_x(0) = 0.0f;  //constraint in negative x-direction
+	_move_constraints_x(1) = 0.0f;  //constraint in positive x-direction
+	_move_constraints_y(0) = 0.0f;  //constraint in negative y-direction
+	_move_constraints_y(1) = 0.0f;  //constraint in positive y-direction
+
+}
+
+void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint)
+{
+
+	collision_constraints_s	constraints;	/**< collision constraints message */
+
+	//fill in values
+	constraints.timestamp = hrt_absolute_time();
+	constraints.constraints_normalized_x[0] = _move_constraints_x_normalized(0);
+	constraints.constraints_normalized_x[1] = _move_constraints_x_normalized(1);
+	constraints.constraints_normalized_y[0] = _move_constraints_y_normalized(0);
+	constraints.constraints_normalized_y[1] = _move_constraints_y_normalized(1);
+
+	constraints.original_setpoint[0] = original_setpoint(0);
+	constraints.original_setpoint[1] = original_setpoint(1);
+	constraints.adapted_setpoint[0] = adapted_setpoint(0);
+	constraints.adapted_setpoint[1] = adapted_setpoint(1);
+
+	// publish constraints
+	if (_constraints_pub != nullptr) {
+		orb_publish(ORB_ID(collision_constraints), _constraints_pub, &constraints);
+
+	} else {
+		_constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints);
+	}
+
+}
+
+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
+
+		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);
+				//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);
+
+				if (vel_lim_x > 0 && vel_lim_x > _move_constraints_x_normalized(1)) { _move_constraints_x_normalized(1) = vel_lim_x; }
+
+				if (vel_lim_y > 0 && vel_lim_y > _move_constraints_y_normalized(1)) { _move_constraints_y_normalized(1) = vel_lim_y; }
+
+				if (vel_lim_x < 0 && -vel_lim_x > _move_constraints_x_normalized(0)) { _move_constraints_x_normalized(0) = -vel_lim_x; }
+
+				if (vel_lim_y < 0 && -vel_lim_y > _move_constraints_y_normalized(0)) { _move_constraints_y_normalized(0) = -vel_lim_y; }
+			}
+		}
+
+	} else if (_last_message + MESSAGE_THROTTLE_US < hrt_absolute_time()) {
+		mavlink_log_critical(&_mavlink_log_pub, "No range data received");
+		_last_message = hrt_absolute_time();
+	}
+}
+
+void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float max_speed)
+{
+
+	reset_constraints();
+
+	//calculate movement constraints based on range data
+	update_range_constraints();
+	_move_constraints_x = _move_constraints_x_normalized;
+	_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 v_max_x, v_max_y;
+
+	if (vel_mag > 0.0f) {
+		v_max_x = abs(max_speed / vel_mag * original_setpoint(0));
+		v_max_y = abs(max_speed / vel_mag * original_setpoint(1));
+
+	} else {
+		v_max_x = 0.0f;
+		v_max_y = 0.0f;
+	}
+
+	//scale the velocity reductions with the maximum possible velocity along the respective axis
+	_move_constraints_x(0) *= v_max_x;
+	_move_constraints_x(1) *= v_max_x;
+	_move_constraints_y(0) *= v_max_y;
+	_move_constraints_y(1) *= v_max_y;
+
+	//apply the velocity reductions to form velocity limits
+	_move_constraints_x(0) = v_max_x - _move_constraints_x(0);
+	_move_constraints_x(1) = v_max_x - _move_constraints_x(1);
+	_move_constraints_y(0) = v_max_y - _move_constraints_y(0);
+	_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;
+	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));
+
+	//warn user if collision avoidance starts to interfere
+	bool currently_interfering = (new_setpoint(0) < 0.95f * original_setpoint(0)
+				      || new_setpoint(0) > 1.05f * original_setpoint(0) || new_setpoint(1) < 0.95f * original_setpoint(1)
+				      || new_setpoint(1) > 1.05f * original_setpoint(1));
+
+	if (currently_interfering && (currently_interfering != _interfering)) {
+		mavlink_log_critical(&_mavlink_log_pub, "Collision Avoidance starts interfering");
+	}
+
+	_interfering = currently_interfering;
+
+	publish_constraints(original_setpoint, new_setpoint);
+	original_setpoint = new_setpoint;
+}

src/lib/CollisionAvoidance/CollisionAvoidance.hpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file CollisionAvoidance.hpp
+ * @author Tanja Baumann <tanja@auterion.com>
+ *
+ * CollisionAvoidance controller.
+ *
+ */
+
+#pragma once
+
+#include <px4_module_params.h>
+#include <float.h>
+#include <matrix/matrix/math.hpp>
+#include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/collision_constraints.h>
+#include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/mavlink_log.h>
+#include <uORB/Publication.hpp>
+using uORB::Publication;
+#include <uORB/uORB.h>
+#include <systemlib/mavlink_log.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+class CollisionAvoidance : public ModuleParams
+{
+public:
+	CollisionAvoidance();
+
+	~CollisionAvoidance() = default;
+
+	void activate() {_is_active = true;}
+
+	void deactivate() {_is_active = false;}
+
+	bool is_active() {return _is_active;}
+
+	bool collision_avoidance_enabled() { return MPC_COL_AVOID.get(); }
+
+	void update(const obstacle_distance_s &distance_measurements) {_obstacle_distance = distance_measurements;}
+
+	void update_range_constraints();
+
+	void reset_constraints();
+
+	void publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint);
+
+	void modifySetpoint(Vector2f &original_setpoint, const float max_speed);
+
+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 */
+
+	static constexpr uint64_t RANGE_STREAM_TIMEOUT_US = 500000;
+	static constexpr uint64_t MESSAGE_THROTTLE_US = 5_s;
+
+	hrt_abstime _last_message;
+
+	Vector2f _move_constraints_x_normalized;
+	Vector2f _move_constraints_y_normalized;
+	Vector2f _move_constraints_x;
+	Vector2f _move_constraints_y;
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::MPC_COL_AVOID>) MPC_COL_AVOID, /**< use range sensor measurements to avoid collision */
+		(ParamFloat<px4::params::MPC_COL_AVOID_D>) MPC_COL_AVOID_D /**< collision avoidance keep minimum distance */
+	)
+
+};

src/lib/FlightTasks/FlightTasks.hpp

@@ -45,6 +45,7 @@
 #include "SubscriptionArray.hpp"
 #include "FlightTasks_generated.hpp"
 #include <lib/WeatherVane/WeatherVane.hpp>
+#include <lib/CollisionAvoidance/CollisionAvoidance.hpp>
 
 #include <new>
 
@@ -131,6 +132,11 @@ 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,6 +52,7 @@
 #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
@@ -154,6 +155,12 @@ 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

@@ -113,6 +113,12 @@ 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);
+	}
+
 	_velocity_setpoint(0) = vel_sp_xy(0);
 	_velocity_setpoint(1) = vel_sp_xy(1);
 }

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

@@ -56,6 +56,12 @@ 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 */
 	void _updateSetpoints() override;
@@ -74,4 +80,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*/
 };

src/modules/logger/logger.cpp

@@ -622,6 +622,7 @@ void Logger::add_default_topics()
 	add_topic("battery_status", 500);
 	add_topic("camera_capture");
 	add_topic("camera_trigger");
+	add_topic("collision_constraints");
 	add_topic("cpuload");
 	add_topic("distance_sensor", 100);
 	add_topic("ekf2_innovations", 200);
@@ -633,6 +634,7 @@ void Logger::add_default_topics()
 	add_topic("manual_control_setpoint", 200);
 	add_topic("mission");
 	add_topic("mission_result");
+	add_topic("obstacle_distance");
 	add_topic("optical_flow", 50);
 	add_topic("position_setpoint_triplet", 200);
 	add_topic("radio_status");

src/modules/mc_pos_control/CMakeLists.txt

@@ -45,4 +45,5 @@ px4_add_module(
 		git_ecl
 		ecl_geo
 		WeatherVane
+		CollisionAvoidance
 	)

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -47,6 +47,7 @@
 #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>
@@ -64,6 +65,7 @@
 
 #include <lib/FlightTasks/FlightTasks.hpp>
 #include <lib/WeatherVane/WeatherVane.hpp>
+#include <lib/CollisionAvoidance/CollisionAvoidance.hpp>
 #include "PositionControl.hpp"
 #include "Utility/ControlMath.hpp"
 
@@ -117,6 +119,7 @@ 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 */
 
@@ -133,6 +136,7 @@ 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 */
@@ -181,6 +185,7 @@ 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.
@@ -355,6 +360,9 @@ MulticopterPositionControl::~MulticopterPositionControl()
 	if (_wv_controller != nullptr) {
 		delete _wv_controller;
 	}
+	if (_ca_controller != nullptr) {
+		delete _ca_controller;
+	}
 }
 
 void
@@ -425,6 +433,10 @@ 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);
@@ -465,6 +477,12 @@ 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
@@ -582,6 +600,7 @@ 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);
 
@@ -644,6 +663,20 @@ MulticopterPositionControl::run()
 			_wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw);
 		}
 
+		// activate the collision avoidance if required. If activated a flighttask can use it modify the setpoint
+		if (_ca_controller != nullptr) {
+
+			// in manual mode we just want to use weathervane if position is controlled as well
+			if (_ca_controller->collision_avoidance_enabled()) {
+				_ca_controller->activate();
+
+			} else {
+				_ca_controller->deactivate();
+			}
+
+			_ca_controller->update(_obstacle_distance);
+		}
+
 		if (_control_mode.flag_armed) {
 			// as soon vehicle is armed check for flighttask
 			start_flight_task();
@@ -665,6 +698,7 @@ 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

@@ -737,3 +737,22 @@ PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0);
  * @group Mission
  */
 PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);
+
+/**
+ * Flag to enable the use of a range sensor for collision avoidance.
+ *
+ * @boolean
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_INT32(MPC_COL_AVOID, 0);
+/**
+ * Minimum Obstacle Distance at which the vehicle should not get closer
+ *
+ * Only relevant if in Position control and the range sensor is active
+ *
+ * @min 0
+ * @max 15
+ * @unit meters
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_COL_AVOID_D, 4.0f);