CollisionPrevention move orb subscriptions to uORB::Subscription

src/lib/CollisionPrevention/CollisionPrevention.cpp

@@ -56,20 +56,10 @@ CollisionPrevention::~CollisionPrevention()
 	}
 }
 
-bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscription_array)
-{
-	if (!subscription_array.get(ORB_ID(obstacle_distance), _sub_obstacle_distance)) {
-		return false;
-	}
-
-	return true;
-}
-
 void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_setpoint,
 		const Vector2f &adapted_setpoint)
 {
-
+	collision_constraints_s	constraints{};	/**< collision constraints message */
-	collision_constraints_s	constraints;	/**< collision constraints message */
 
 	//fill in values
 	constraints.timestamp = hrt_absolute_time();
@@ -91,7 +81,8 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se
 void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint,
 		const Vector2f &curr_pos, const Vector2f &curr_vel)
 {
-	const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance->get();
+	_sub_obstacle_distance.update();
+	const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
 
 	//The maximum velocity formula contains a square root, therefore the whole calculation is done with squared norms.
 	//that way the root does not have to be calculated for every range bin but once at the end.

src/lib/CollisionPrevention/CollisionPrevention.hpp

@@ -49,9 +49,8 @@
 #include <mathlib/mathlib.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/mavlink_log.h>
-#include <uORB/uORB.h>
+#include <uORB/Subscription.hpp>
 #include <systemlib/mavlink_log.h>
-#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
 
 class CollisionPrevention : public ModuleParams
 {
@@ -60,12 +59,6 @@ public:
 
 	~CollisionPrevention();
 
-	/**
-	 * Initialize the uORB subscriptions using an array
-	 * @return true on success, false on error
-	 */
-	bool initializeSubscriptions(SubscriptionArray &subscription_array);
-
 	bool is_active() { return _param_mpc_col_prev_d.get() > 0; }
 
 	void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
@@ -78,7 +71,7 @@ private:
 	orb_advert_t _constraints_pub{nullptr};  	/**< constraints publication */
 	orb_advert_t _mavlink_log_pub{nullptr};	 	/**< Mavlink log uORB handle */
 
-	uORB::SubscriptionPollable<obstacle_distance_s> *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */
+	uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
 
 	static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500000};
 	static constexpr uint64_t MESSAGE_THROTTLE_US{5000000};

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

@@ -52,10 +52,6 @@ bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscr
 		return false;
 	}
 
-	if (!_collision_prevention.initializeSubscriptions(subscription_array)) {
-		return false;
-	}
-
 	return true;
 }