ColisionAvoidance: move instantiation to FlightTask

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