Add a collision avoidance library which uses range data

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*/
 };