FlightTasks: switch to uORB::Subscription

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 = -3
+	ActivationFailed = -2
 };
 
 class FlightTasks
@@ -169,23 +167,21 @@ class FlightTasks
 	};
 	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)
-	    || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.lon)) {
+	if (!PX4_ISFINITE(_sub_triplet_setpoint.get().current.lat)
+	    || !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) {
-			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));
-			_triplet_prev_wp(2) = -(_sub_triplet_setpoint->get().previous.alt - _reference_altitude);
+		if (_isFinite(_sub_triplet_setpoint.get().previous) && _sub_triplet_setpoint.get().previous.valid) {
+			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));
+			_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) {
-			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));
-			_triplet_next_wp(2) = -(_sub_triplet_setpoint->get().next.alt - _reference_altitude);
+		} 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,
+					       _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);
 
 		} 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) {
-		_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
+	} else if (_type == WaypointType::follow_target && _sub_triplet_setpoint.get().current.yawspeed_valid) {
+		_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
 		_yaw_setpoint = NAN;
 
 	} else {
-		if (_sub_triplet_setpoint->get().current.yaw_valid) {
-			_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
+		if (_sub_triplet_setpoint.get().current.yaw_valid) {
+			_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.yawspeed_valid ? _sub_triplet_setpoint->get().next.yawspeed : NAN,
-				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint->get().current.type);
+				_sub_triplet_setpoint.get().next.yaw,
+				_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);
 		_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) {
-			v = Vector2f(&_sub_home_position->get().x) - Vector2f(_position);
+		if (_sub_home_position.get().valid_hpos) {
+			v = Vector2f(&_sub_home_position.get().x) - Vector2f(_position);
 		}
 
 		break;
 
 	case 2: // Heading point away from home.
-		if (_sub_home_position->get().valid_hpos) {
-			v = Vector2f(_position) - Vector2f(&_sub_home_position->get().x);
+		if (_sub_home_position.get().valid_hpos) {
+			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_lon =  _sub_vehicle_local_position->get().ref_lon;
-	_reference_altitude = _sub_vehicle_local_position->get().ref_alt;
+	double ref_lat =  _sub_vehicle_local_position.get().ref_lat;
+	double ref_lon =  _sub_vehicle_local_position.get().ref_lon;
+	_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_lon)) {
+	    && PX4_ISFINITE(_sub_vehicle_local_position.get().ref_lat)
+	    && 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 vy = _sub_triplet_setpoint->get().current.vy;
+	const float vx = _sub_triplet_setpoint.get().current.vx;
+	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 @@ class FlightTaskAuto : public FlightTask
 	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 @@ class FlightTaskAuto : public FlightTask
 	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::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr};
+
+	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
+	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 @@ class FlightTaskAuto : public FlightTask
 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;