FlightTasks: switch to uORB::Subscription

- delete SubscriptionArray
 - all FlightTasks are now responsible for updating their own subscriptions (typically in updateInitialize()).

src/lib/FlightTasks/FlightTasks.cpp

@@ -20,7 +20,6 @@ bool FlightTasks::update()
 	_updateCommand();
 
 	if (isAnyTaskActive()) {
-		_subscription_array.update();
 		return _current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize();
 	}
 
@@ -77,16 +76,6 @@ FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index)
 		return FlightTaskError::NoError;
 	}
 
-	// subscription failed
-	if (!_current_task.task->initializeSubscriptions(_subscription_array)) {
-		_current_task.task->~FlightTask();
-		_current_task.task = nullptr;
-		_current_task.index = FlightTaskIndex::None;
-		return FlightTaskError::SubscriptionFailed;
-	}
-
-	_subscription_array.forcedUpdate(); // make sure data is available for all new subscriptions
-
 	// activation failed
 	if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) {
 		_current_task.task->~FlightTask();

src/lib/FlightTasks/FlightTasks.hpp

@@ -42,7 +42,6 @@
 #pragma once
 
 #include "FlightTask.hpp"
-#include "SubscriptionArray.hpp"
 #include "FlightTasks_generated.hpp"
 #include <lib/WeatherVane/WeatherVane.hpp>
 #include <uORB/PublicationQueued.hpp>
@@ -55,8 +54,7 @@
 enum class FlightTaskError : int {
 	NoError = 0,
 	InvalidTask = -1,
-	SubscriptionFailed = -2,
+	ActivationFailed = -2
-	ActivationFailed = -3
 };
 
 class FlightTasks
@@ -169,23 +167,21 @@ private:
 	};
 	flight_task_t _current_task = {nullptr, FlightTaskIndex::None};
 
-	SubscriptionArray _subscription_array;
-
 	struct task_error_t {
 		int error;
 		const char *msg;
 	};
 
-	static const int _numError = 4;
+	static constexpr int _numError = 3;
 	/**
 	 * Map from Error int to user friendly string.
 	 */
 	task_error_t _taskError[_numError] = {
 		{static_cast<int>(FlightTaskError::NoError), "No Error"},
 		{static_cast<int>(FlightTaskError::InvalidTask), "Invalid Task "},
-		{static_cast<int>(FlightTaskError::SubscriptionFailed), "Subscription Failed"},
 		{static_cast<int>(FlightTaskError::ActivationFailed), "Activation Failed"}
 	};
+
 	/**
 	 * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them
 	 */

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -48,35 +48,6 @@ FlightTaskAuto::FlightTaskAuto() :
 
 }
 
-bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_array)
-{
-	if (!FlightTask::initializeSubscriptions(subscription_array)) {
-		return false;
-	}
-
-	if (!subscription_array.get(ORB_ID(position_setpoint_triplet), _sub_triplet_setpoint)) {
-		return false;
-	}
-
-	if (!subscription_array.get(ORB_ID(home_position), _sub_home_position)) {
-		return false;
-	}
-
-	if (!subscription_array.get(ORB_ID(vehicle_status), _sub_vehicle_status)) {
-		return false;
-	}
-
-	if (!subscription_array.get(ORB_ID(manual_control_setpoint), _sub_manual_control_setpoint)) {
-		return false;
-	}
-
-	if (!_obstacle_avoidance.initializeSubscriptions(subscription_array)) {
-		return false;
-	}
-
-	return true;
-}
-
 bool FlightTaskAuto::activate(vehicle_local_position_setpoint_s last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
@@ -91,6 +62,12 @@ bool FlightTaskAuto::activate(vehicle_local_position_setpoint_s last_setpoint)
 bool FlightTaskAuto::updateInitialize()
 {
 	bool ret = FlightTask::updateInitialize();
+
+	_sub_home_position.update();
+	_sub_manual_control_setpoint.update();
+	_sub_vehicle_status.update();
+	_sub_triplet_setpoint.update();
+
 	// require valid reference and valid target
 	ret = ret && _evaluateGlobalReference() && _evaluateTriplets();
 	// require valid position
@@ -159,17 +136,17 @@ bool FlightTaskAuto::_evaluateTriplets()
 
 	// Check if triplet is valid. There must be at least a valid altitude.
 
-	if (!_sub_triplet_setpoint->get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.alt)) {
+	if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) {
 		// Best we can do is to just set all waypoints to current state and return false.
 		_prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position;
 		_type = WaypointType::position;
 		return false;
 	}
 
-	_type = (WaypointType)_sub_triplet_setpoint->get().current.type;
+	_type = (WaypointType)_sub_triplet_setpoint.get().current.type;
 
 	// Always update cruise speed since that can change without waypoint changes.
-	_mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed;
+	_mc_cruise_speed = _sub_triplet_setpoint.get().current.cruising_speed;
 
 	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) {
 		// If no speed is planned use the default cruise speed as limit
@@ -182,8 +159,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 	// Temporary target variable where we save the local reprojection of the latest navigator current triplet.
 	Vector3f tmp_target;
 
-	if (!PX4_ISFINITE(_sub_triplet_setpoint->get().current.lat)
+	if (!PX4_ISFINITE(_sub_triplet_setpoint.get().current.lat)
-	    || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.lon)) {
+	    || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.lon)) {
 		// No position provided in xy. Lock position
 		if (!PX4_ISFINITE(_lock_position_xy(0))) {
 			tmp_target(0) = _lock_position_xy(0) = _position(0);
@@ -200,10 +177,10 @@ bool FlightTaskAuto::_evaluateTriplets()
 
 		// Convert from global to local frame.
 		map_projection_project(&_reference_position,
-				       _sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &tmp_target(0), &tmp_target(1));
+				       _sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon, &tmp_target(0), &tmp_target(1));
 	}
 
-	tmp_target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude);
+	tmp_target(2) = -(_sub_triplet_setpoint.get().current.alt - _reference_altitude);
 
 	// Check if anything has changed. We do that by comparing the temporary target
 	// to the internal _triplet_target.
@@ -222,7 +199,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 
 	} else {
 		_triplet_target = tmp_target;
-		_target_acceptance_radius = _sub_triplet_setpoint->get().current.acceptance_radius;
+		_target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius;
 
 		if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) {
 			// Horizontal target is not finite.
@@ -237,10 +214,10 @@ bool FlightTaskAuto::_evaluateTriplets()
 		// If _triplet_target has updated, update also _triplet_prev_wp and _triplet_next_wp.
 		_prev_prev_wp = _triplet_prev_wp;
 
-		if (_isFinite(_sub_triplet_setpoint->get().previous) && _sub_triplet_setpoint->get().previous.valid) {
+		if (_isFinite(_sub_triplet_setpoint.get().previous) && _sub_triplet_setpoint.get().previous.valid) {
-			map_projection_project(&_reference_position, _sub_triplet_setpoint->get().previous.lat,
+			map_projection_project(&_reference_position, _sub_triplet_setpoint.get().previous.lat,
-					       _sub_triplet_setpoint->get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1));
+					       _sub_triplet_setpoint.get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1));
-			_triplet_prev_wp(2) = -(_sub_triplet_setpoint->get().previous.alt - _reference_altitude);
+			_triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude);
 
 		} else {
 			_triplet_prev_wp = _position;
@@ -249,10 +226,10 @@ bool FlightTaskAuto::_evaluateTriplets()
 		if (_type == WaypointType::loiter) {
 			_triplet_next_wp = _triplet_target;
 
-		} else if (_isFinite(_sub_triplet_setpoint->get().next) && _sub_triplet_setpoint->get().next.valid) {
+		} else if (_isFinite(_sub_triplet_setpoint.get().next) && _sub_triplet_setpoint.get().next.valid) {
-			map_projection_project(&_reference_position, _sub_triplet_setpoint->get().next.lat,
+			map_projection_project(&_reference_position, _sub_triplet_setpoint.get().next.lat,
-					       _sub_triplet_setpoint->get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1));
+					       _sub_triplet_setpoint.get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1));
-			_triplet_next_wp(2) = -(_sub_triplet_setpoint->get().next.alt - _reference_altitude);
+			_triplet_next_wp(2) = -(_sub_triplet_setpoint.get().next.alt - _reference_altitude);
 
 		} else {
 			_triplet_next_wp = _triplet_target;
@@ -261,7 +238,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 
 	if (_ext_yaw_handler != nullptr) {
 		// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
-		(_param_wv_en.get() && _sub_triplet_setpoint->get().current.allow_weather_vane) ?	_ext_yaw_handler->activate() :
+		(_param_wv_en.get() && _sub_triplet_setpoint.get().current.allow_weather_vane) ?	_ext_yaw_handler->activate() :
 		_ext_yaw_handler->deactivate();
 	}
 
@@ -270,13 +247,13 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_yaw_setpoint = _yaw;
 		_yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate();
 
-	} else if (_type == WaypointType::follow_target && _sub_triplet_setpoint->get().current.yawspeed_valid) {
+	} else if (_type == WaypointType::follow_target && _sub_triplet_setpoint.get().current.yawspeed_valid) {
-		_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
+		_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
 		_yaw_setpoint = NAN;
 
 	} else {
-		if (_sub_triplet_setpoint->get().current.yaw_valid) {
+		if (_sub_triplet_setpoint.get().current.yaw_valid) {
-			_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
+			_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
 
 		} else {
 			_set_heading_from_mode();
@@ -291,16 +268,16 @@ bool FlightTaskAuto::_evaluateTriplets()
 
 	if (triplet_update || (_current_state != previous_state)) {
 		_updateInternalWaypoints();
-		_mission_gear = _sub_triplet_setpoint->get().current.landing_gear;
+		_mission_gear = _sub_triplet_setpoint.get().current.landing_gear;
 	}
 
 	if (_param_com_obs_avoid.get()
-	    && _sub_vehicle_status->get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	    && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 		_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
 				_triplet_next_wp,
-				_sub_triplet_setpoint->get().next.yaw,
+				_sub_triplet_setpoint.get().next.yaw,
-				_sub_triplet_setpoint->get().next.yawspeed_valid ? _sub_triplet_setpoint->get().next.yawspeed : NAN,
+				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : NAN,
-				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint->get().current.type);
+				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
 		_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
 	}
 
@@ -320,15 +297,15 @@ void FlightTaskAuto::_set_heading_from_mode()
 		break;
 
 	case 1: // Heading points towards home.
-		if (_sub_home_position->get().valid_hpos) {
+		if (_sub_home_position.get().valid_hpos) {
-			v = Vector2f(&_sub_home_position->get().x) - Vector2f(_position);
+			v = Vector2f(&_sub_home_position.get().x) - Vector2f(_position);
 		}
 
 		break;
 
 	case 2: // Heading point away from home.
-		if (_sub_home_position->get().valid_hpos) {
+		if (_sub_home_position.get().valid_hpos) {
-			v = Vector2f(_position) - Vector2f(&_sub_home_position->get().x);
+			v = Vector2f(_position) - Vector2f(&_sub_home_position.get().x);
 		}
 
 		break;
@@ -372,22 +349,22 @@ bool FlightTaskAuto::_evaluateGlobalReference()
 	// Only update if reference timestamp has changed AND no valid reference altitude
 	// is available.
 	// TODO: this needs to be revisited and needs a more clear implementation
-	if (_sub_vehicle_local_position->get().ref_timestamp == _time_stamp_reference && PX4_ISFINITE(_reference_altitude)) {
+	if (_sub_vehicle_local_position.get().ref_timestamp == _time_stamp_reference && PX4_ISFINITE(_reference_altitude)) {
 		// don't need to update anything
 		return true;
 	}
 
-	double ref_lat =  _sub_vehicle_local_position->get().ref_lat;
+	double ref_lat =  _sub_vehicle_local_position.get().ref_lat;
-	double ref_lon =  _sub_vehicle_local_position->get().ref_lon;
+	double ref_lon =  _sub_vehicle_local_position.get().ref_lon;
-	_reference_altitude = _sub_vehicle_local_position->get().ref_alt;
+	_reference_altitude = _sub_vehicle_local_position.get().ref_alt;
 
-	if (!_sub_vehicle_local_position->get().z_global) {
+	if (!_sub_vehicle_local_position.get().z_global) {
 		// we have no valid global altitude
 		// set global reference to local reference
 		_reference_altitude = 0.0f;
 	}
 
-	if (!_sub_vehicle_local_position->get().xy_global) {
+	if (!_sub_vehicle_local_position.get().xy_global) {
 		// we have no valid global alt/lat
 		// set global reference to local reference
 		ref_lat = 0.0;
@@ -401,8 +378,8 @@ bool FlightTaskAuto::_evaluateGlobalReference()
 
 	// check if everything is still finite
 	if (PX4_ISFINITE(_reference_altitude)
-	    && PX4_ISFINITE(_sub_vehicle_local_position->get().ref_lat)
+	    && PX4_ISFINITE(_sub_vehicle_local_position.get().ref_lat)
-	    && PX4_ISFINITE(_sub_vehicle_local_position->get().ref_lon)) {
+	    && PX4_ISFINITE(_sub_vehicle_local_position.get().ref_lon)) {
 		return true;
 
 	} else {
@@ -424,10 +401,10 @@ void FlightTaskAuto::_setDefaultConstraints()
 Vector2f FlightTaskAuto::_getTargetVelocityXY()
 {
 	// guard against any bad velocity values
-	const float vx = _sub_triplet_setpoint->get().current.vx;
+	const float vx = _sub_triplet_setpoint.get().current.vx;
-	const float vy = _sub_triplet_setpoint->get().current.vy;
+	const float vy = _sub_triplet_setpoint.get().current.vy;
 	bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) &&
-			      _sub_triplet_setpoint->get().current.velocity_valid;
+			      _sub_triplet_setpoint.get().current.velocity_valid;
 
 	if (velocity_valid) {
 		return Vector2f(vx, vy);

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp

@@ -77,7 +77,6 @@ public:
 	FlightTaskAuto();
 
 	virtual ~FlightTaskAuto() = default;
-	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
 	bool updateInitialize() override;
 	bool updateFinalize() override;
@@ -99,9 +98,10 @@ protected:
 	matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
 	float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
 	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
-	uORB::SubscriptionPollable<home_position_s> *_sub_home_position{nullptr};
+
-	uORB::SubscriptionPollable<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
+	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
-	uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr};
+	uORB::SubscriptionData<manual_control_setpoint_s>	_sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
+	uORB::SubscriptionData<vehicle_status_s>		_sub_vehicle_status{ORB_ID(vehicle_status)};
 
 	State _current_state{State::none};
 	float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
@@ -127,7 +127,8 @@ protected:
 private:
 	matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
 	bool _yaw_lock{false}; /**< if within acceptance radius, lock yaw to current yaw */
-	uORB::SubscriptionPollable<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
+
+	uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
 
 	matrix::Vector3f
 	_triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */

src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -161,9 +161,9 @@ void FlightTaskAutoMapper::_updateAltitudeAboveGround()
 		// We have a valid distance to ground measurement
 		_alt_above_ground = _dist_to_bottom;
 
-	} else if (_sub_home_position->get().valid_alt) {
+	} else if (_sub_home_position.get().valid_alt) {
 		// if home position is set, then altitude above ground is relative to the home position
-		_alt_above_ground = -_position(2) + _sub_home_position->get().z;
+		_alt_above_ground = -_position(2) + _sub_home_position.get().z;
 	}
 }
 

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -176,9 +176,9 @@ void FlightTaskAutoMapper2::_updateAltitudeAboveGround()
 		// We have a valid distance to ground measurement
 		_alt_above_ground = _dist_to_bottom;
 
-	} else if (_sub_home_position->get().valid_alt) {
+	} else if (_sub_home_position.get().valid_alt) {
 		// if home position is set, then altitude above ground is relative to the home position
-		_alt_above_ground = -_position(2) + _sub_home_position->get().z;
+		_alt_above_ground = -_position(2) + _sub_home_position.get().z;
 	}
 }
 
@@ -199,12 +199,12 @@ bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
 float FlightTaskAutoMapper2::_getLandSpeed()
 {
 	bool rc_assist_enabled = _param_mpc_land_rc_help.get();
-	bool rc_is_valid = !_sub_vehicle_status->get().rc_signal_lost;
+	bool rc_is_valid = !_sub_vehicle_status.get().rc_signal_lost;
 
 	float throttle = 0.5f;
 
 	if (rc_is_valid && rc_assist_enabled) {
-		throttle = _sub_manual_control_setpoint->get().z;
+		throttle = _sub_manual_control_setpoint.get().z;
 	}
 
 	float speed = 0;

src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt

@@ -33,7 +33,6 @@
 
 px4_add_library(FlightTask
     FlightTask.cpp
-    SubscriptionArray.cpp
 )
 
 target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp

@@ -9,19 +9,6 @@ const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NA
 const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
 const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
 
-bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
-{
-	if (!subscription_array.get(ORB_ID(vehicle_local_position), _sub_vehicle_local_position)) {
-		return false;
-	}
-
-	if (!subscription_array.get(ORB_ID(vehicle_attitude), _sub_attitude)) {
-		return false;
-	}
-
-	return true;
-}
-
 bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint)
 {
 	_resetSetpoints();
@@ -43,6 +30,10 @@ bool FlightTask::updateInitialize()
 	_time = (_time_stamp_current - _time_stamp_activate) / 1e6f;
 	_deltatime  = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
 	_time_stamp_last = _time_stamp_current;
+
+	_sub_vehicle_local_position.update();
+	_sub_attitude.update();
+
 	_evaluateVehicleLocalPosition();
 	_checkEkfResetCounters();
 	return true;
@@ -50,40 +41,40 @@ bool FlightTask::updateInitialize()
 
 void FlightTask::_initEkfResetCounters()
 {
-	_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
+	_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
-	_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
+	_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
-	_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
+	_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
-	_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
+	_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
-	_reset_counters.quat = _sub_attitude->get().quat_reset_counter;
+	_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
 }
 
 void FlightTask::_checkEkfResetCounters()
 {
 	// Check if a reset event has happened
-	if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
+	if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) {
 		_ekfResetHandlerPositionXY();
-		_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
+		_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
 	}
 
-	if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
+	if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) {
 		_ekfResetHandlerVelocityXY();
-		_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
+		_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
 	}
 
-	if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
+	if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) {
 		_ekfResetHandlerPositionZ();
-		_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
+		_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
 	}
 
-	if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) {
+	if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
 		_ekfResetHandlerVelocityZ();
-		_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
+		_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
 	}
 
-	if (_sub_attitude->get().quat_reset_counter != _reset_counters.quat) {
+	if (_sub_attitude.get().quat_reset_counter != _reset_counters.quat) {
-		float delta_psi = matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi();
+		float delta_psi = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().delta_q_reset)).psi();
 		_ekfResetHandlerHeading(delta_psi);
-		_reset_counters.quat = _sub_attitude->get().quat_reset_counter;
+		_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
 	}
 }
 
@@ -133,44 +124,44 @@ void FlightTask::_evaluateVehicleLocalPosition()
 	_yaw = NAN;
 	_dist_to_bottom = NAN;
 
-	if ((_time_stamp_current - _sub_attitude->get().timestamp) < _timeout) {
+	if ((_time_stamp_current - _sub_attitude.get().timestamp) < _timeout) {
 		// yaw
-		_yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude->get().q)).psi();
+		_yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().q)).psi();
 	}
 
 	// Only use vehicle-local-position topic fields if the topic is received within a certain timestamp
-	if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {
+	if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) {
 
 		// position
-		if (_sub_vehicle_local_position->get().xy_valid) {
+		if (_sub_vehicle_local_position.get().xy_valid) {
-			_position(0) = _sub_vehicle_local_position->get().x;
+			_position(0) = _sub_vehicle_local_position.get().x;
-			_position(1) = _sub_vehicle_local_position->get().y;
+			_position(1) = _sub_vehicle_local_position.get().y;
 		}
 
-		if (_sub_vehicle_local_position->get().z_valid) {
+		if (_sub_vehicle_local_position.get().z_valid) {
-			_position(2) = _sub_vehicle_local_position->get().z;
+			_position(2) = _sub_vehicle_local_position.get().z;
 		}
 
 		// velocity
-		if (_sub_vehicle_local_position->get().v_xy_valid) {
+		if (_sub_vehicle_local_position.get().v_xy_valid) {
-			_velocity(0) = _sub_vehicle_local_position->get().vx;
+			_velocity(0) = _sub_vehicle_local_position.get().vx;
-			_velocity(1) = _sub_vehicle_local_position->get().vy;
+			_velocity(1) = _sub_vehicle_local_position.get().vy;
 		}
 
-		if (_sub_vehicle_local_position->get().v_z_valid) {
+		if (_sub_vehicle_local_position.get().v_z_valid) {
-			_velocity(2) = _sub_vehicle_local_position->get().vz;
+			_velocity(2) = _sub_vehicle_local_position.get().vz;
 		}
 
 		// distance to bottom
-		if (_sub_vehicle_local_position->get().dist_bottom_valid
+		if (_sub_vehicle_local_position.get().dist_bottom_valid
-		    && PX4_ISFINITE(_sub_vehicle_local_position->get().dist_bottom)) {
+		    && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) {
-			_dist_to_bottom =  _sub_vehicle_local_position->get().dist_bottom;
+			_dist_to_bottom =  _sub_vehicle_local_position.get().dist_bottom;
 		}
 
 		// global frame reference coordinates to enable conversions
-		if (_sub_vehicle_local_position->get().xy_global && _sub_vehicle_local_position->get().z_global) {
+		if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) {
-			globallocalconverter_init(_sub_vehicle_local_position->get().ref_lat, _sub_vehicle_local_position->get().ref_lon,
+			globallocalconverter_init(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
-						  _sub_vehicle_local_position->get().ref_alt, _sub_vehicle_local_position->get().ref_timestamp);
+						  _sub_vehicle_local_position.get().ref_alt, _sub_vehicle_local_position.get().ref_timestamp);
 		}
 	}
 }
@@ -194,7 +185,7 @@ bool FlightTask::_checkTakeoff()
 	if (PX4_ISFINITE(_position_setpoint(2))) {
 		// minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow
 		float min_altitude = 0.2f;
-		const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;
+		const float min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
 
 		if (PX4_ISFINITE(min_distance_to_ground)) {
 			min_altitude = min_distance_to_ground + 0.05f;

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -44,7 +44,7 @@
 #include <px4_module_params.h>
 #include <drivers/drv_hrt.h>
 #include <matrix/matrix/math.hpp>
-#include <uORB/SubscriptionPollable.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -53,7 +53,6 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include <lib/WeatherVane/WeatherVane.hpp>
-#include "SubscriptionArray.hpp"
 
 class FlightTask : public ModuleParams
 {
@@ -67,13 +66,6 @@ public:
 
 	virtual ~FlightTask() = default;
 
-	/**
-	 * Initialize the uORB subscriptions using an array
-	 * @param subscription_array handling uORB subscribtions externally across task switches
-	 * @return true on success, false on error
-	 */
-	virtual bool initializeSubscriptions(SubscriptionArray &subscription_array);
-
 	/**
 	 * Call once on the event where you switch to the task
 	 * @param state of the previous task
@@ -176,8 +168,8 @@ public:
 
 protected:
 
-	uORB::SubscriptionPollable<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
+	uORB::SubscriptionData<vehicle_local_position_s>	_sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
-	uORB::SubscriptionPollable<vehicle_attitude_s> *_sub_attitude{nullptr};
+	uORB::SubscriptionData<vehicle_attitude_s>		_sub_attitude{ORB_ID(vehicle_attitude)};
 
 	/** Reset all setpoints to NAN */
 	void _resetSetpoints();

src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.cpp

@@ -1,85 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SubscriptionArray.hpp"
-
-#include <string.h>
-
-SubscriptionArray::~SubscriptionArray()
-{
-	cleanup();
-}
-
-void SubscriptionArray::cleanup()
-{
-	for (int i = 0; i < _subscriptions_count; ++i) {
-		delete _subscriptions[i];
-	}
-
-	delete[] _subscriptions;
-	_subscriptions = nullptr;
-}
-
-bool SubscriptionArray::resizeSubscriptions()
-{
-	const int new_size = _subscriptions_size == 0 ? 4 : _subscriptions_size * 2;
-	uORB::SubscriptionPollableNode **new_array = new uORB::SubscriptionPollableNode*[new_size];
-
-	if (!new_array) {
-		return false;
-	}
-
-	if (_subscriptions) {
-		memcpy(new_array, _subscriptions, sizeof(uORB::SubscriptionPollableNode *)*_subscriptions_count);
-		delete[] _subscriptions;
-	}
-
-	_subscriptions = new_array;
-	_subscriptions_size = new_size;
-
-	return true;
-}
-
-void SubscriptionArray::update()
-{
-	for (int i = 0; i < _subscriptions_count; ++i) {
-		_subscriptions[i]->update();
-	}
-}
-
-void SubscriptionArray::forcedUpdate()
-{
-	for (int i = 0; i < _subscriptions_count; ++i) {
-		_subscriptions[i]->forcedUpdate();
-	}
-}

src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp

@@ -1,112 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file SubscriptionArray.hpp
- *
- * Simple array that contains a dynamic amount of Subscription<T> instances
- *
- * @author Beat Küng <beat-kueng@gmx.net>
- */
-
-#pragma once
-
-#include <uORB/SubscriptionPollable.hpp>
-
-class SubscriptionArray
-{
-public:
-	SubscriptionArray() = default;
-	~SubscriptionArray();
-
-	/**
-	 * Get a subscription
-	 * @param meta ORB_ID(topic)
-	 * @param subscription returned subscription (output parameter)
-	 * @param instance topic instance
-	 * @return true on success, false otherwise (subscription set to nullptr)
-	 */
-	template<class T>
-	bool get(const struct orb_metadata *meta, uORB::SubscriptionPollable<T> *&subscription, unsigned instance = 0);
-
-	/**
-	 * update all subscriptions (if new data is available)
-	 */
-	void update();
-
-	/**
-	 * update all subscriptions
-	 */
-	void forcedUpdate();
-
-private:
-	void cleanup();
-
-	bool resizeSubscriptions();
-
-	uORB::SubscriptionPollableNode **_subscriptions{nullptr};
-	int _subscriptions_count{0}; ///< number of valid subscriptions
-	int _subscriptions_size{0}; ///< actual size of the _subscriptions array
-};
-
-
-template<class T>
-bool SubscriptionArray::get(const struct orb_metadata *meta, uORB::SubscriptionPollable<T> *&subscription,
-			    unsigned instance)
-{
-	// does it already exist?
-	for (int i = 0; i < _subscriptions_count; ++i) {
-		if (_subscriptions[i]->getMeta() == meta && _subscriptions[i]->getInstance() == instance) {
-			// we know the type must be correct, so we can use reinterpret_cast (dynamic_cast is not available)
-			subscription = reinterpret_cast<uORB::SubscriptionPollable<T>*>(_subscriptions[i]);
-			return true;
-		}
-	}
-
-	// resize if needed
-	if (_subscriptions_count >= _subscriptions_size) {
-		if (!resizeSubscriptions()) {
-			subscription = nullptr;
-			return false;
-		}
-	}
-
-	subscription = new uORB::SubscriptionPollable<T>(meta, 0, instance);
-
-	if (!subscription) {
-		return false;
-	}
-
-	_subscriptions[_subscriptions_count++] = subscription;
-	return true;
-}

src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp

@@ -42,22 +42,12 @@
 using namespace matrix;
 using namespace time_literals;
 
-bool FlightTaskManual::initializeSubscriptions(SubscriptionArray &subscription_array)
-{
-	if (!FlightTask::initializeSubscriptions(subscription_array)) {
-		return false;
-	}
-
-	if (!subscription_array.get(ORB_ID(manual_control_setpoint), _sub_manual_control_setpoint)) {
-		return false;
-	}
-
-	return true;
-}
-
 bool FlightTaskManual::updateInitialize()
 {
 	bool ret = FlightTask::updateInitialize();
+
+	_sub_manual_control_setpoint.update();
+
 	const bool sticks_available = _evaluateSticks();
 
 	if (_sticks_data_required) {
@@ -72,13 +62,13 @@ bool FlightTaskManual::_evaluateSticks()
 	hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
 
 	/* Sticks are rescaled linearly and exponentially to [-1,1] */
-	if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < rc_timeout) {
+	if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
 
 		/* Linear scale  */
-		_sticks(0) = _sub_manual_control_setpoint->get().x; /* NED x, "pitch" [-1,1] */
+		_sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */
-		_sticks(1) = _sub_manual_control_setpoint->get().y; /* NED y, "roll" [-1,1] */
+		_sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */
-		_sticks(2) = -(_sub_manual_control_setpoint->get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */
+		_sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */
-		_sticks(3) = _sub_manual_control_setpoint->get().r; /* "yaw" [-1,1] */
+		_sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */
 
 		/* Exponential scale */
 		_sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
@@ -89,7 +79,7 @@ bool FlightTaskManual::_evaluateSticks()
 		// Only switch the landing gear up if the user switched from gear down to gear up.
 		// If the user had the switch in the gear up position and took off ignore it
 		// until he toggles the switch to avoid retracting the gear immediately on takeoff.
-		int8_t gear_switch = _sub_manual_control_setpoint->get().gear_switch;
+		int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
 
 		if (_gear_switch_old != gear_switch) {
 			_applyGearSwitch(gear_switch);

src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp

@@ -50,7 +50,6 @@ public:
 
 	virtual ~FlightTaskManual() = default;
 
-	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 	bool applyCommandParameters(const vehicle_command_s &command) override { return FlightTask::applyCommandParameters(command); };
 	bool updateInitialize() override;
 
@@ -68,7 +67,7 @@ private:
 	bool _evaluateSticks(); /**< checks and sets stick inputs */
 	void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */
 
-	uORB::SubscriptionPollable<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
+	uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
 					(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -41,22 +41,12 @@
 
 using namespace matrix;
 
-bool FlightTaskManualAltitude::initializeSubscriptions(SubscriptionArray &subscription_array)
-{
-	if (!FlightTaskManual::initializeSubscriptions(subscription_array)) {
-		return false;
-	}
-
-	if (!subscription_array.get(ORB_ID(home_position), _sub_home_position)) {
-		return false;
-	}
-
-	return true;
-}
-
 bool FlightTaskManualAltitude::updateInitialize()
 {
 	bool ret = FlightTaskManual::updateInitialize();
+
+	_sub_home_position.update();
+
 	// in addition to manual require valid position and velocity in D-direction and valid yaw
 	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
 }
@@ -73,15 +63,15 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s
 
 	_constraints.tilt = math::radians(_param_mpc_man_tilt_max.get());
 
-	if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_min)) {
+	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) {
-		_constraints.min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;
+		_constraints.min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
 
 	} else {
 		_constraints.min_distance_to_ground = -INFINITY;
 	}
 
-	if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_max)) {
+	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) {
-		_constraints.max_distance_to_ground = _sub_vehicle_local_position->get().hagl_max;
+		_constraints.max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;
 
 	} else {
 		_constraints.max_distance_to_ground = INFINITY;
@@ -182,9 +172,9 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 		} else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) {
 			// Position is locked but check if a reset event has happened.
 			// We will shift the setpoints.
-			if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counter) {
+			if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counter) {
 				_position_setpoint(2) = _position(2);
-				_reset_counter = _sub_vehicle_local_position->get().z_reset_counter;
+				_reset_counter = _sub_vehicle_local_position.get().z_reset_counter;
 			}
 
 		} else  {
@@ -273,8 +263,8 @@ void FlightTaskManualAltitude::_respectGroundSlowdown()
 	if (PX4_ISFINITE(_dist_to_bottom)) {
 		dist_to_ground = _dist_to_bottom;
 
-	} else if (_sub_home_position->get().valid_alt) {
+	} else if (_sub_home_position.get().valid_alt) {
-		dist_to_ground = -(_position(2) - _sub_home_position->get().z);
+		dist_to_ground = -(_position(2) - _sub_home_position.get().z);
 	}
 
 	// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -47,7 +47,6 @@ class FlightTaskManualAltitude : public FlightTaskManual
 public:
 	FlightTaskManualAltitude() = default;
 	virtual ~FlightTaskManualAltitude() = default;
-	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;
@@ -111,7 +110,7 @@ private:
 	 */
 	void _respectGroundSlowdown();
 
-	uORB::SubscriptionPollable<home_position_s> *_sub_home_position{nullptr};
+	uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
 
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
 	float _max_speed_up = 10.0f;

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

@@ -46,15 +46,6 @@ FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(thi
 
 }
 
-bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscription_array)
-{
-	if (!FlightTaskManualAltitude::initializeSubscriptions(subscription_array)) {
-		return false;
-	}
-
-	return true;
-}
-
 bool FlightTaskManualPosition::updateInitialize()
 {
 	bool ret = FlightTaskManualAltitude::updateInitialize();
@@ -102,11 +93,11 @@ void FlightTaskManualPosition::_scaleSticks()
 	}
 
 	// scale the stick inputs
-	if (PX4_ISFINITE(_sub_vehicle_local_position->get().vxy_max)) {
+	if (PX4_ISFINITE(_sub_vehicle_local_position.get().vxy_max)) {
 		// estimator provides vehicle specific max
 
 		// use the minimum of the estimator and user specified limit
-		_velocity_scale = fminf(_constraints.speed_xy, _sub_vehicle_local_position->get().vxy_max);
+		_velocity_scale = fminf(_constraints.speed_xy, _sub_vehicle_local_position.get().vxy_max);
 		// Allow for a minimum of 0.3 m/s for repositioning
 		_velocity_scale = fmaxf(_velocity_scale, 0.3f);
 
@@ -152,10 +143,10 @@ void FlightTaskManualPosition::_updateXYlock()
 	} else if (PX4_ISFINITE(_position_setpoint(0)) && apply_brake) {
 		// Position is locked but check if a reset event has happened.
 		// We will shift the setpoints.
-		if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) {
+		if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counter) {
 			_position_setpoint(0) = _position(0);
 			_position_setpoint(1) = _position(1);
-			_reset_counter = _sub_vehicle_local_position->get().xy_reset_counter;
+			_reset_counter = _sub_vehicle_local_position.get().xy_reset_counter;
 		}
 
 	} else {

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

@@ -49,7 +49,6 @@ public:
 	FlightTaskManualPosition();
 
 	virtual ~FlightTaskManualPosition() = default;
-	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
 	bool updateInitialize() override;
 

src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp

@@ -39,24 +39,15 @@
 
 using namespace matrix;
 
-bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription_array)
-{
-	if (!FlightTask::initializeSubscriptions(subscription_array)) {
-		return false;
-	}
-
-	if (!subscription_array.get(ORB_ID(position_setpoint_triplet), _sub_triplet_setpoint)) {
-		return false;
-	}
-
-	return true;
-}
-
 bool FlightTaskOffboard::updateInitialize()
 {
 	bool ret = FlightTask::updateInitialize();
+
+	_sub_triplet_setpoint.update();
+
 	// require a valid triplet
-	ret = ret && _sub_triplet_setpoint->get().current.valid;
+	ret = ret && _sub_triplet_setpoint.get().current.valid;
+
 	// require valid position / velocity in xy
 	return ret && PX4_ISFINITE(_position(0))
 	       && PX4_ISFINITE(_position(1))
@@ -78,32 +69,32 @@ bool FlightTaskOffboard::update()
 	// reset setpoint for every loop
 	_resetSetpoints();
 
-	if (!_sub_triplet_setpoint->get().current.valid) {
+	if (!_sub_triplet_setpoint.get().current.valid) {
 		_setDefaultConstraints();
 		_position_setpoint = _position;
 		return false;
 	}
 
 	// Yaw / Yaw-speed
-	if (_sub_triplet_setpoint->get().current.yaw_valid) {
+	if (_sub_triplet_setpoint.get().current.yaw_valid) {
 		// yaw control required
-		_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
+		_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
 
-		if (_sub_triplet_setpoint->get().current.yawspeed_valid) {
+		if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
 			// yawspeed is used as feedforward
-			_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
+			_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
 		}
 
-	} else if (_sub_triplet_setpoint->get().current.yawspeed_valid) {
+	} else if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
 		// only yawspeed required
-		_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
+		_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
 		// set yaw setpoint to NAN since not used
 		_yaw_setpoint = NAN;
 
 	}
 
 	// Loiter
-	if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 		// loiter just means that the vehicle should keep position
 		if (!PX4_ISFINITE(_position_lock(0))) {
 			_position_setpoint = _position_lock = _position;
@@ -120,7 +111,7 @@ bool FlightTaskOffboard::update()
 	}
 
 	// Takeoff
-	if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
+	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 		// just do takeoff to default altitude
 		if (!PX4_ISFINITE(_position_lock(0))) {
 			_position_setpoint = _position_lock = _position;
@@ -138,7 +129,7 @@ bool FlightTaskOffboard::update()
 	}
 
 	// Land
-	if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 		// land with landing speed, but keep position in xy
 		if (!PX4_ISFINITE(_position_lock(0))) {
 			_position_setpoint = _position_lock = _position;
@@ -158,7 +149,7 @@ bool FlightTaskOffboard::update()
 	}
 
 	// IDLE
-	if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 		_thrust_setpoint.zero();
 		return true;
 	}
@@ -168,17 +159,17 @@ bool FlightTaskOffboard::update()
 	// 2. position setpoint + velocity setpoint (velocity used as feedforward)
 	// 3. velocity setpoint
 	// 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported
-	const bool position_ctrl_xy = _sub_triplet_setpoint->get().current.position_valid
+	const bool position_ctrl_xy = _sub_triplet_setpoint.get().current.position_valid
-				      && _sub_vehicle_local_position->get().xy_valid;
+				      && _sub_vehicle_local_position.get().xy_valid;
-	const bool position_ctrl_z = _sub_triplet_setpoint->get().current.alt_valid
+	const bool position_ctrl_z = _sub_triplet_setpoint.get().current.alt_valid
-				     && _sub_vehicle_local_position->get().z_valid;
+				     && _sub_vehicle_local_position.get().z_valid;
-	const bool velocity_ctrl_xy = _sub_triplet_setpoint->get().current.velocity_valid
+	const bool velocity_ctrl_xy = _sub_triplet_setpoint.get().current.velocity_valid
-				      && _sub_vehicle_local_position->get().v_xy_valid;
+				      && _sub_vehicle_local_position.get().v_xy_valid;
-	const bool velocity_ctrl_z = _sub_triplet_setpoint->get().current.velocity_valid
+	const bool velocity_ctrl_z = _sub_triplet_setpoint.get().current.velocity_valid
-				     && _sub_vehicle_local_position->get().v_z_valid;
+				     && _sub_vehicle_local_position.get().v_z_valid;
 	const bool feedforward_ctrl_xy = position_ctrl_xy && velocity_ctrl_xy;
 	const bool feedforward_ctrl_z = position_ctrl_z && velocity_ctrl_z;
-	const bool acceleration_ctrl = _sub_triplet_setpoint->get().current.acceleration_valid;
+	const bool acceleration_ctrl = _sub_triplet_setpoint.get().current.acceleration_valid;
 
 	// if nothing is valid in xy, then exit offboard
 	if (!(position_ctrl_xy || velocity_ctrl_xy || acceleration_ctrl)) {
@@ -192,29 +183,29 @@ bool FlightTaskOffboard::update()
 
 	// XY-direction
 	if (feedforward_ctrl_xy) {
-		_position_setpoint(0) = _sub_triplet_setpoint->get().current.x;
+		_position_setpoint(0) = _sub_triplet_setpoint.get().current.x;
-		_position_setpoint(1) = _sub_triplet_setpoint->get().current.y;
+		_position_setpoint(1) = _sub_triplet_setpoint.get().current.y;
-		_velocity_setpoint(0) = _sub_triplet_setpoint->get().current.vx;
+		_velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
-		_velocity_setpoint(1) = _sub_triplet_setpoint->get().current.vy;
+		_velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
 
 	} else if (position_ctrl_xy) {
-		_position_setpoint(0) = _sub_triplet_setpoint->get().current.x;
+		_position_setpoint(0) = _sub_triplet_setpoint.get().current.x;
-		_position_setpoint(1) = _sub_triplet_setpoint->get().current.y;
+		_position_setpoint(1) = _sub_triplet_setpoint.get().current.y;
 
 	} else if (velocity_ctrl_xy) {
 
-		if (_sub_triplet_setpoint->get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
+		if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
 			// in local frame: don't require any transformation
-			_velocity_setpoint(0) = _sub_triplet_setpoint->get().current.vx;
+			_velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
-			_velocity_setpoint(1) = _sub_triplet_setpoint->get().current.vy;
+			_velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
 
-		} else if (_sub_triplet_setpoint->get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
+		} else if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
 			// in body frame: need to transorm first
 			// Note, this transformation is wrong because body-xy is not neccessarily on the same plane as locale-xy
-			_velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint->get().current.vx - sinf(
+			_velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint.get().current.vx - sinf(
-							_yaw) * _sub_triplet_setpoint->get().current.vy;
+							_yaw) * _sub_triplet_setpoint.get().current.vy;
-			_velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint->get().current.vx + cosf(
+			_velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint.get().current.vx + cosf(
-							_yaw) * _sub_triplet_setpoint->get().current.vy;
+							_yaw) * _sub_triplet_setpoint.get().current.vy;
 
 		} else {
 			// no valid frame
@@ -224,22 +215,22 @@ bool FlightTaskOffboard::update()
 
 	// Z-direction
 	if (feedforward_ctrl_z) {
-		_position_setpoint(2) = _sub_triplet_setpoint->get().current.z;
+		_position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
-		_velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz;
+		_velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
 
 	} else if (position_ctrl_z) {
-		_position_setpoint(2) = _sub_triplet_setpoint->get().current.z;
+		_position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
 
 	} else if (velocity_ctrl_z) {
-		_velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz;
+		_velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
 	}
 
 	// Acceleration
 	// Note: this is not supported yet and will be mapped to normalized thrust directly.
-	if (_sub_triplet_setpoint->get().current.acceleration_valid) {
+	if (_sub_triplet_setpoint.get().current.acceleration_valid) {
-		_thrust_setpoint(0) = _sub_triplet_setpoint->get().current.a_x;
+		_thrust_setpoint(0) = _sub_triplet_setpoint.get().current.a_x;
-		_thrust_setpoint(1) = _sub_triplet_setpoint->get().current.a_y;
+		_thrust_setpoint(1) = _sub_triplet_setpoint.get().current.a_y;
-		_thrust_setpoint(2) = _sub_triplet_setpoint->get().current.a_z;
+		_thrust_setpoint(2) = _sub_triplet_setpoint.get().current.a_z;
 	}
 
 	// use default conditions of upwards position or velocity to take off

src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp

@@ -47,13 +47,13 @@ public:
 	FlightTaskOffboard() = default;
 
 	virtual ~FlightTaskOffboard() = default;
-	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 	bool update() override;
 	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
 	bool updateInitialize() override;
 
 protected:
-	uORB::SubscriptionPollable<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
+	uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
+
 private:
 	matrix::Vector3f _position_lock{};
 

src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp

@@ -56,32 +56,21 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
 
 }
 
-bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array)
-{
-	if (!subscription_array.get(ORB_ID(vehicle_trajectory_waypoint), _sub_vehicle_trajectory_waypoint)) {
-		return false;
-	}
-
-	if (!subscription_array.get(ORB_ID(vehicle_status), _sub_vehicle_status)) {
-		return false;
-	}
-
-	return true;
-}
-
 void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp,
 		float &yaw_speed_sp)
 {
+	_sub_vehicle_status.update();
+	_sub_vehicle_trajectory_waypoint.update();
 
-	if (_sub_vehicle_status->get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
+	if (_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
 		// if in failsafe nav_state LOITER, don't inject setpoints from avoidance system
 		return;
 	}
 
-	const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint->get().timestamp)
+	const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint.get().timestamp)
 					    > TRAJECTORY_STREAM_TIMEOUT_US;
 	const bool avoidance_point_valid =
-		_sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
+		_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
 
 	_avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid, hrt_absolute_time());
 
@@ -106,13 +95,13 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 	}
 
 	if (avoidance_point_valid) {
-		pos_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position;
+		pos_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position;
-		vel_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity;
+		vel_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity;
 
 		if (!_ext_yaw_active) {
 			// inject yaw setpoints only if weathervane isn't active
-			yaw_sp =  _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
+			yaw_sp =  _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
-			yaw_speed_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
+			yaw_speed_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
 		}
 	}
 

src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp

@@ -48,6 +48,7 @@
 
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationQueued.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_status.h>
@@ -58,8 +59,6 @@
 
 #include <matrix/matrix/math.hpp>
 
-#include <SubscriptionArray.hpp>
-
 const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
 	{	{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
 		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
@@ -75,8 +74,6 @@ public:
 	ObstacleAvoidance(ModuleParams *parent);
 	~ObstacleAvoidance() = default;
 
-	bool initializeSubscriptions(SubscriptionArray &subscription_array);
-
 	/**
 	 * Inject setpoints from obstacle avoidance system into FlightTasks.
 	 * @param pos_sp, position setpoint
@@ -119,8 +116,8 @@ public:
 
 protected:
 
-	uORB::SubscriptionPollable<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr}; /**< vehicle trajectory waypoint subscription */
+	uORB::SubscriptionData<vehicle_trajectory_waypoint_s> _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */
-	uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */
+	uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
 
 	vehicle_trajectory_waypoint_s _desired_waypoint{};  /**< desired vehicle trajectory waypoint to be sent to OA */
 

src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp

@@ -42,7 +42,6 @@ using namespace matrix;
 class ObstacleAvoidanceTest : public ::testing::Test
 {
 public:
-	SubscriptionArray subscription_array;
 	Vector3f pos_sp;
 	Vector3f vel_sp;
 	float yaw_sp = 0.123f;
@@ -71,7 +70,6 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy)
 	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message coming
 	// from offboard
 	TestObstacleAvoidance oa;
-	oa.initializeSubscriptions(subscription_array);
 
 	vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint;
 	message.timestamp = hrt_absolute_time();
@@ -91,8 +89,6 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy)
 	orb_advert_t vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &vehicle_status);
 	orb_publish(ORB_ID(vehicle_status), vehicle_status_pub, &vehicle_status);
 
-	subscription_array.update();
-
 	// WHEN: we inject the vehicle_trajectory_waypoint in the interface
 	oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
 
@@ -109,7 +105,6 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy)
 {
 	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message
 	TestObstacleAvoidance oa;
-	oa.initializeSubscriptions(subscription_array);
 
 	vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint;
 	Vector3f pos(3.1f, 4.7f, 5.2f);
@@ -124,8 +119,6 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy)
 	orb_advert_t vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &vehicle_status);
 	orb_publish(ORB_ID(vehicle_status), vehicle_status_pub, &vehicle_status);
 
-	subscription_array.update();
-
 	// WHEN: we inject the vehicle_trajectory_waypoint in the interface
 	oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
 
@@ -142,7 +135,6 @@ TEST_F(ObstacleAvoidanceTest, oa_desired)
 {
 	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and the waypoints from FLightTaskAuto
 	TestObstacleAvoidance oa;
-	oa.initializeSubscriptions(subscription_array);
 
 	pos_sp = Vector3f(1.f, 1.2f, NAN);
 	vel_sp = Vector3f(NAN, NAN, 0.7f);