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