FlightTasks: move FlightTask to own folder/lib

fix header includes
add cmake subdirectories dynamically

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

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018 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.
+#
+############################################################################
+
+px4_add_library(FlightTask
+    FlightTask.cpp
+    SubscriptionArray.cpp
+)
+
+target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

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

@@ -0,0 +1,146 @@
+#include "FlightTask.hpp"
+#include <mathlib/mathlib.h>
+#include <lib/ecl/geo/geo.h>
+
+constexpr uint64_t FlightTask::_timeout;
+// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
+const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};
+const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, vehicle_constraints_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()
+{
+	_resetSetpoints();
+	_setDefaultConstraints();
+	_time_stamp_activate = hrt_absolute_time();
+	_heading_reset_counter = _sub_attitude->get().quat_reset_counter;
+	return true;
+}
+
+bool FlightTask::updateInitialize()
+{
+	_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;
+	return _evaluateVehicleLocalPosition();
+}
+
+const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
+{
+	/* fill position setpoint message */
+	vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
+	vehicle_local_position_setpoint.timestamp = hrt_absolute_time();
+
+	vehicle_local_position_setpoint.x = _position_setpoint(0);
+	vehicle_local_position_setpoint.y = _position_setpoint(1);
+	vehicle_local_position_setpoint.z = _position_setpoint(2);
+
+	vehicle_local_position_setpoint.vx = _velocity_setpoint(0);
+	vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
+	vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
+
+	vehicle_local_position_setpoint.acc_x = _acceleration_setpoint(0);
+	vehicle_local_position_setpoint.acc_y = _acceleration_setpoint(1);
+	vehicle_local_position_setpoint.acc_z = _acceleration_setpoint(2);
+
+	_thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
+	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
+	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
+
+	return vehicle_local_position_setpoint;
+}
+
+void FlightTask::_resetSetpoints()
+{
+	_position_setpoint *= NAN;
+	_velocity_setpoint *= NAN;
+	_acceleration_setpoint *= NAN;
+	_thrust_setpoint *= NAN;
+	_yaw_setpoint = _yawspeed_setpoint = NAN;
+}
+
+bool FlightTask::_evaluateVehicleLocalPosition()
+{
+	if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {
+
+		// position
+		if (_sub_vehicle_local_position->get().xy_valid) {
+			_position(0) = _sub_vehicle_local_position->get().x;
+			_position(1) = _sub_vehicle_local_position->get().y;
+
+		} else {
+			_position(0) = _position(1) = NAN;
+		}
+
+		if (_sub_vehicle_local_position->get().z_valid) {
+			_position(2) = _sub_vehicle_local_position->get().z;
+
+		} else {
+			_position(2) = NAN;
+		}
+
+		// velocity
+		if (_sub_vehicle_local_position->get().v_xy_valid) {
+			_velocity(0) = _sub_vehicle_local_position->get().vx;
+			_velocity(1) = _sub_vehicle_local_position->get().vy;
+
+		} else {
+			_velocity(0) = _velocity(1) = NAN;
+		}
+
+		if (_sub_vehicle_local_position->get().v_z_valid) {
+			_velocity(2) = _sub_vehicle_local_position->get().vz;
+
+		} else {
+			_velocity(2) = NAN;
+		}
+
+		// yaw
+		_yaw = _sub_vehicle_local_position->get().yaw;
+
+		// distance to bottom
+		_dist_to_bottom = NAN;
+
+		if (_sub_vehicle_local_position->get().dist_bottom_valid
+		    && PX4_ISFINITE(_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) {
+			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);
+		}
+
+		// We don't check here if states are valid or not.
+		// Validity checks are done in the sub-classes.
+		return true;
+
+	} else {
+		_resetSetpoints();
+		return false;
+	}
+}
+
+void FlightTask::_setDefaultConstraints()
+{
+	_constraints.speed_xy = MPC_XY_VEL_MAX.get();
+	_constraints.speed_up = MPC_Z_VEL_MAX_UP.get();
+	_constraints.speed_down = MPC_Z_VEL_MAX_DN.get();
+	_constraints.tilt = math::radians(MPC_TILTMAX_AIR.get());
+	_constraints.landing_gear = vehicle_constraints_s::GEAR_KEEP;
+	_constraints.min_distance_to_ground = NAN;
+	_constraints.max_distance_to_ground = NAN;
+}

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

@@ -0,0 +1,192 @@
+/****************************************************************************
+ *
+ *   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 FlightTask.hpp
+ *
+ * Abstract base class for different advanced flight tasks like orbit, follow me, ...
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <px4_module_params.h>
+#include <drivers/drv_hrt.h>
+#include <matrix/matrix/math.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_constraints.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include "SubscriptionArray.hpp"
+
+class FlightTask : public ModuleParams
+{
+public:
+	FlightTask() :
+		ModuleParams(nullptr)
+	{
+		_resetSetpoints();
+		_constraints = empty_constraints;
+	}
+
+	virtual ~FlightTask() = default;
+
+	/**
+	 * Initialize the uORB subscriptions using an array
+	 * @return true on success, false on error
+	 */
+	virtual bool initializeSubscriptions(SubscriptionArray &subscription_array);
+
+	/**
+	 * Call once on the event where you switch to the task
+	 * @return true on success, false on error
+	 */
+	virtual bool activate();
+
+	/**
+	 * To be called to adopt parameters from an arrived vehicle command
+	 * @return true if accepted, false if declined
+	 */
+	virtual bool applyCommandParameters(const vehicle_command_s &command) { return true; };
+
+	/**
+	 * Call before activate() or update()
+	 * to initialize time and input data
+	 * @return true on success, false on error
+	 */
+	virtual bool updateInitialize();
+
+	/**
+	 * To be called regularly in the control loop cycle to execute the task
+	 * @return true on success, false on error
+	 */
+	virtual bool update() = 0;
+
+	/**
+	 * Get the output data
+	 */
+	const vehicle_local_position_setpoint_s getPositionSetpoint();
+
+	/**
+	 * Get vehicle constraints.
+	 * The constraints can vary with task.
+	 * @return constraints
+	 */
+	const vehicle_constraints_s getConstraints() {return _constraints;};
+
+	/**
+	 * Empty setpoint.
+	 * All setpoints are set to NAN.
+	 */
+	static const vehicle_local_position_setpoint_s empty_setpoint;
+
+	/**
+	 * Empty constraints.
+	 * All constraints are set to NAN.
+	 */
+	static const vehicle_constraints_s empty_constraints;
+
+	/**
+	 * Call this whenever a parameter update notification is received (parameter_update uORB message)
+	 */
+	void handleParameterUpdate()
+	{
+		updateParams();
+	}
+
+protected:
+
+	uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
+	uORB::Subscription<vehicle_attitude_s> *_sub_attitude{nullptr};
+	uint8_t _heading_reset_counter{0}; /**< estimator heading reset */
+
+	/**
+	 * Reset all setpoints to NAN
+	 */
+	void _resetSetpoints();
+
+	/*
+	 * Check and update local position
+	 */
+	bool _evaluateVehicleLocalPosition();
+
+	/**
+	 * Set constraints to default values
+	 */
+	virtual void  _setDefaultConstraints();
+
+	/* Time abstraction */
+	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
+	float _time = 0; /**< passed time in seconds since the task was activated */
+	float _deltatime = 0; /**< passed time in seconds since the task was last updated */
+	hrt_abstime _time_stamp_activate = 0; /**< time stamp when task was activated */
+	hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */
+	hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */
+
+	/* Current vehicle state */
+	matrix::Vector3f _position; /**< current vehicle position */
+	matrix::Vector3f _velocity; /**< current vehicle velocity */
+	float _yaw = 0.f; /**< current vehicle yaw heading */
+	float _dist_to_bottom = 0.0f; /**< current height above ground level */
+
+	/**
+	 * Setpoints which the position controller has to execute.
+	 * Setpoints that are set to NAN are not controlled. Not all setpoints can be set at the same time.
+	 * If more than one type of setpoint is set, then order of control is a as follow: position, velocity,
+	 * acceleration, thrust. The exception is _position_setpoint together with _velocity_setpoint, where the
+	 * _velocity_setpoint is used as feedforward.
+	 * _acceleration_setpoint is currently not supported.
+	 */
+	matrix::Vector3f _position_setpoint;
+	matrix::Vector3f _velocity_setpoint;
+	matrix::Vector3f _acceleration_setpoint;
+	matrix::Vector3f _thrust_setpoint;
+	float _yaw_setpoint;
+	float _yawspeed_setpoint;
+
+	/**
+	 * Vehicle constraints.
+	 * The constraints can vary with tasks.
+	 */
+	vehicle_constraints_s _constraints;
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
+					(ParamFloat<px4::params::MPC_XY_VEL_MAX>) MPC_XY_VEL_MAX,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) MPC_Z_VEL_MAX_DN,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) MPC_Z_VEL_MAX_UP,
+					(ParamFloat<px4::params::MPC_TILTMAX_AIR>) MPC_TILTMAX_AIR
+				       )
+};

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

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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();
+	}
+}
+
+void SubscriptionArray::forcedUpdate()
+{
+	for (int i = 0; i < _subscriptions_count; ++i) {
+		_subscriptions[i]->forcedUpdate();
+	}
+}

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

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   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 (if new data is available)
+	 */
+	void update();
+
+	/**
+	 * update all subscriptions
+	 */
+	void forcedUpdate();
+
+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]->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::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;
+}