FlightTasks: add SubscriptionArray class that contains all subscriptions

This is to avoid dynamic (re-)allocations on task switching, due to
orb_{un,}subscribe calls.

src/lib/FlightTasks/CMakeLists.txt

@@ -36,6 +36,7 @@ px4_add_module(
 		tasks/FlightTask.cpp
 		tasks/FlightTaskManual.cpp
 		tasks/FlightTaskOrbit.cpp
+		SubscriptionArray.cpp
 	DEPENDS
 		platforms__common
 		modules__uORB

src/lib/FlightTasks/FlightTasks.hpp

@@ -45,6 +45,8 @@
 #include "tasks/FlightTaskManual.hpp"
 #include "tasks/FlightTaskOrbit.hpp"
 
+#include "SubscriptionArray.hpp"
+
 #include <new>
 
 class FlightTasks : control::SuperBlock
@@ -68,6 +70,7 @@ public:
 	int update()
 	{
 		if (is_any_task_active()) {
+			_subscription_array.update();
 			return _current_task->update();
 		}
 
@@ -132,6 +135,13 @@ public:
 			return 1;
 		}
 
+		if (!_current_task->initializeSubscriptions(_subscription_array)) {
+			_current_task->~FlightTask();
+			_current_task = nullptr;
+			_current_task_index = -1;
+			return 1;
+		}
+
 		_current_task_index = task_number;
 		_current_task->update();
 		return 0;
@@ -172,4 +182,6 @@ private:
 
 	FlightTask *_current_task = nullptr;
 	int _current_task_index = -1;
+
+	SubscriptionArray _subscription_array;
 };

src/lib/FlightTasks/SubscriptionArray.cpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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::SubscriptionNode **new_array = new uORB::SubscriptionNode*[new_size];
+
+	if (!new_array) {
+		return false;
+	}
+
+	if (_subscriptions) {
+		memcpy(new_array, _subscriptions, sizeof(uORB::SubscriptionNode *)*_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();
+	}
+}

src/lib/FlightTasks/SubscriptionArray.hpp

@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   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/Subscription.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::Subscription<T> *&subscription, unsigned instance = 0);
+
+	/**
+	 * update all subscriptions
+	 */
+	void update();
+
+private:
+	void cleanup();
+
+	bool resizeSubscriptions();
+
+	uORB::SubscriptionNode **_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::Subscription<T> *&subscription, unsigned instance)
+{
+	// does it already exist?
+	for (int i = 0; i < _subscriptions_count; ++i) {
+		if (_subscriptions[i]->meta() == meta && _subscriptions[i]->instance() == instance) {
+			// we know the type must be correct, so we can use reinterpret_cast (dynamic_cast is not available)
+			subscription = reinterpret_cast<uORB::Subscription<T>*>(_subscriptions[i]);
+			return true;
+		}
+	}
+
+	// resize if needed
+	if (_subscriptions_count >= _subscriptions_size) {
+		if (!resizeSubscriptions()) {
+			subscription = nullptr;
+			return false;
+		}
+	}
+
+	subscription = new uORB::Subscription<T>(meta, 0, instance);
+
+	if (!subscription) {
+		return false;
+	}
+
+	_subscriptions[_subscriptions_count++] = subscription;
+	return true;
+}

src/lib/FlightTasks/tasks/FlightTask.cpp

@@ -3,23 +3,32 @@
 
 constexpr uint64_t FlightTask::_timeout;
 
+
+bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
+{
+	if (!subscription_array.get(ORB_ID(vehicle_local_position), _sub_vehicle_local_position)) {
+		return false;
+	}
+
+	return true;
+}
+
 int FlightTask::update()
 {
 	_time_stamp_current = hrt_absolute_time();
 	_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;
-	updateSubscriptions();
 	int ret = _evaluate_vehicle_position();
 	return ret;
 }
 
 int FlightTask::_evaluate_vehicle_position()
 {
-	if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) {
-		_position = matrix::Vector3f(&_sub_vehicle_local_position.get().x);
-		_velocity = matrix::Vector3f(&_sub_vehicle_local_position.get().vx);
-		_yaw = _sub_vehicle_local_position.get().yaw;
+	if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {
+		_position = matrix::Vector3f(&_sub_vehicle_local_position->get().x);
+		_velocity = matrix::Vector3f(&_sub_vehicle_local_position->get().vx);
+		_yaw = _sub_vehicle_local_position->get().yaw;
 		return 0;
 
 	} else {

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -47,17 +47,24 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 
+#include "../SubscriptionArray.hpp"
+
 
 class FlightTask : public control::SuperBlock
 {
 public:
 	FlightTask(SuperBlock *parent, const char *name) :
-		SuperBlock(parent, name),
-		_sub_vehicle_local_position(ORB_ID(vehicle_local_position), 0, 0, &getSubscriptions())
+		SuperBlock(parent, name)
 	{
 		_time_stamp_activate = hrt_absolute_time();
 	};
 
+	/**
+	 * initialize the uORB subscriptions using an array
+	 * @return true on success, false on error
+	 */
+	virtual bool initializeSubscriptions(SubscriptionArray &subscription_array);
+
 	virtual ~FlightTask() = default;
 
 	/**
@@ -104,7 +111,7 @@ protected:
 	void _set_yawspeed_setpoint(const float &yawspeed) { _vehicle_local_position_setpoint.yawspeed = yawspeed; };
 
 private:
-	uORB::Subscription<vehicle_local_position_s> _sub_vehicle_local_position;
+	uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
 
 	vehicle_local_position_setpoint_s _vehicle_local_position_setpoint; /**< Output position setpoint that every task has */
 

src/lib/FlightTasks/tasks/FlightTaskManual.cpp

@@ -48,7 +48,6 @@ using namespace matrix;
 
 FlightTaskManual::FlightTaskManual(SuperBlock *parent, const char *name) :
 	FlightTask(parent, name),
-	_sub_manual_control_setpoint(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()),
 	_xy_vel_man_expo(parent, "MPC_XY_MAN_EXPO", false),
 	_z_vel_man_expo(parent, "MPC_Z_MAN_EXPO", false),
 	_hold_dz(parent, "MPC_HOLD_DZ", false),
@@ -76,6 +75,18 @@ FlightTaskManual::FlightTaskManual(SuperBlock *parent, const char *name) :
 	_hold_position = Vector3f(NAN, NAN, NAN);
 };
 
+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;
+}
 int FlightTaskManual::update()
 {
 	int ret = FlightTask::update();
@@ -135,12 +146,12 @@ int FlightTaskManual::update()
 
 int FlightTaskManual::_evaluate_sticks()
 {
-	if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < _timeout) {
+	if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < _timeout) {
 		/* get data and scale correctly */
-		_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(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(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(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] */
 
 		/* apply expo and deadzone */
 		_sticks(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get());

src/lib/FlightTasks/tasks/FlightTaskManual.hpp

@@ -54,6 +54,8 @@ public:
 
 	virtual ~FlightTaskManual() = default;
 
+	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
+
 	int update() override;
 
 protected:
@@ -62,7 +64,7 @@ protected:
 	float get_input_frame_yaw() { return _yaw; };
 
 private:
-	uORB::Subscription<manual_control_setpoint_s> _sub_manual_control_setpoint;
+	uORB::Subscription<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
 
 	control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */
 	control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */

src/modules/uORB/Subscription.hpp

@@ -83,6 +83,10 @@ public:
 
 	int getHandle() const { return _handle; }
 
+	const orb_metadata *meta() const { return _meta; }
+
+	unsigned instance() const { return _instance; }
+
 protected:
 	const struct orb_metadata *_meta;
 	int _handle;