diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index efb9e97822..fb304d2360 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/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. diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 4c9e5a3711..f75fc8e5c0 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -49,9 +49,8 @@ #include #include #include -#include +#include #include -#include 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 *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */ + uORB::SubscriptionData _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}; diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 8437a6355f..6b9c9c0d59 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/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; }