FlightTasks: added FlightTaskManual as replacement for the current stick based position controlled flight

it's a draft and only works for velocity setpoints oriented in NED frame yet

# Conflicts:
#	src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp

src/lib/FlightTasks/CMakeLists.txt

@@ -34,6 +34,7 @@ px4_add_module(
 	MODULE lib__flight_tasks
 	SRCS
 		FlightTask.h
+		tasks/FlightTaskManual.cpp
 		tasks/FlightTaskOrbit.cpp
 	DEPENDS
 		platforms__common

src/lib/FlightTasks/FlightTasks.hpp

@@ -42,6 +42,7 @@
 #pragma once
 
 #include "tasks/FlightTask.hpp"
+#include "tasks/FlightTaskManual.hpp"
 #include "tasks/FlightTaskOrbit.hpp"
 
 class FlightTasks
@@ -114,7 +115,12 @@ public:
 	 * Get result of the task execution
 	 * @return pointer to the setpoint for the position controller
 	 */
-	const vehicle_local_position_setpoint_s *get_position_setpoint() const { return Orbit.get_position_setpoint(); };
+	const vehicle_local_position_setpoint_s *get_position_setpoint() const
+	{
+		//if (is_any_task_active()) {
+		return _tasks[_current_task]->get_position_setpoint();
+		//}
+	};
 
 	/**
 	 * Check if any task is active
@@ -123,10 +129,11 @@ public:
 	bool is_any_task_active() const { return _current_task > -1 && _current_task < _task_count; };
 
 private:
-	static const int _task_count = 1;
 	int _current_task = -1;
 
+	FlightTaskManual Manual;
 	FlightTaskOrbit Orbit;
-	FlightTask *_tasks[_task_count] = {&Orbit};
+	static const int _task_count = 2;
+	FlightTask *_tasks[_task_count] = {&Manual, &Orbit};
 
 };

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file FlightTask.h
+ * @file FlightTask.hpp
  *
  * Abstract base class for different advanced flight tasks like orbit, follow me, ...
  *
@@ -67,7 +67,7 @@ public:
 	 * Call once on the event of switching away from the task
 	 * 	@return 0 on success, >0 on error
 	 */
-	virtual int disable() = 0;
+	virtual int disable() { return 0; };
 
 	/**
 	 * To be called regularly in the control loop cycle to execute the task
@@ -167,10 +167,10 @@ private:
 	void _evaluate_sticks()
 	{
 		if (_manual_control_setpoint != NULL && hrt_elapsed_time(&_manual_control_setpoint->timestamp) < _timeout) {
-			_sticks(0) = _manual_control_setpoint->y; /* "roll" [-1,1] */
-			_sticks(1) = _manual_control_setpoint->x; /* "pitch" [-1,1] */
-			_sticks(2) = _manual_control_setpoint->r; /* "yaw" [-1,1] */
-			_sticks(3) = (_manual_control_setpoint->z - 0.5f) * 2.f; /* "thrust" resacaled from [0,1] to [-1,1] */
+			_sticks(0) = _manual_control_setpoint->x; /* NED x, "pitch" [-1,1] */
+			_sticks(1) = _manual_control_setpoint->y; /* NED y, "roll" [-1,1] */
+			_sticks(2) = (_manual_control_setpoint->z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */
+			_sticks(3) = _manual_control_setpoint->r; /* "yaw" [-1,1] */
 
 		} else {
 			_sticks = matrix::Vector<float, 4>(); /* default is all zero */

src/lib/FlightTasks/tasks/FlightTaskManual.hpp

@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   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 FlightTaskManual.hpp
+ *
+ * Flight task for the normal, legacy, manual position controlled flight
+ * where stick inputs map basically to the velocity setpoint
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include "FlightTask.hpp"
+
+class FlightTaskManual : public FlightTask
+{
+public:
+	FlightTaskManual() :
+		_xy_vel_man_expo(nullptr, "MPC_XY_MAN_EXPO"),
+		_z_vel_man_expo(nullptr, "MPC_Z_MAN_EXPO"),
+		_hold_dz(nullptr, "MPC_HOLD_DZ"),
+		_velocity_hor_manual(nullptr, "MPC_VEL_MANUAL"),
+		_z_vel_max_up(nullptr, "MPC_Z_VEL_MAX_UP"),
+		_z_vel_max_down(nullptr, "MPC_Z_VEL_MAX_DN")
+	{};
+	virtual ~FlightTaskManual() {};
+
+	/**
+	 * Call once on the event where you switch to the task
+	 * @return 0 on success, >0 on error otherwise
+	 */
+	virtual int activate()
+	{
+		FlightTask::activate();
+		return 0;
+	};
+
+	/**
+	 * Call once on the event of switching away from the task
+	 * 	@return 0 on success, >0 on error otherwise
+	 */
+	virtual int disable()
+	{
+		FlightTask::disable();
+		return 0;
+	};
+
+	/**
+	 * Call regularly in the control loop cycle to execute the task
+	 * @return 0 on success, >0 on error otherwise
+	 */
+	virtual int update()
+	{
+		FlightTask::update();
+
+		matrix::Vector3f man_vel_sp;
+		man_vel_sp(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get());
+		man_vel_sp(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get());
+		man_vel_sp(2) = -math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get());
+
+		const float man_vel_hor_length = matrix::Vector2f(man_vel_sp.data()).length();
+
+		/* saturate such that magnitude is never larger than 1 */
+		if (man_vel_hor_length > 1.0f) {
+			man_vel_sp(0) /= man_vel_hor_length;
+			man_vel_sp(1) /= man_vel_hor_length;
+		}
+
+		/* prepare yaw to rotate into NED frame */
+		float yaw_input_fame = 0;
+		//float yaw_input_fame = _yaw;
+
+		/* setpoint in NED frame */
+		man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, yaw_input_fame)) * man_vel_sp;
+
+		/* scale smaller than unit length vector to maximal manual speed (m/s) */
+		matrix::Vector3f vel_scale(_velocity_hor_manual.get(),
+					   _velocity_hor_manual.get(),
+					   (man_vel_sp(2) > 0.0f) ? _z_vel_max_down.get() : _z_vel_max_up.get());
+		man_vel_sp = man_vel_sp.emult(vel_scale);
+
+		_set_position_setpoint(matrix::Vector3f(NAN, NAN, NAN));
+		_set_velocity_setpoint(man_vel_sp);
+		return 0;
+	};
+
+private:
+	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 */
+	control::BlockParamFloat _hold_dz; /**< deadzone around the center for the sticks when flying in position mode */
+	control::BlockParamFloat _velocity_hor_manual; /**< target velocity in manual controlled mode at full speed */
+	control::BlockParamFloat _z_vel_max_up; /**< maximal vertical velocity when flying upwards with the stick */
+	control::BlockParamFloat _z_vel_max_down; /**< maximal vertical velocity when flying downwards with the stick */
+
+};

src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file FlightTaskOrbit.h
+ * @file FlightTaskOrbit.hpp
  *
  * Flight task for orbiting in a circle around a target position
  *
@@ -63,7 +63,11 @@ public:
 	 * Call once on the event of switching away from the task
 	 * 	@return 0 on success, >0 on error otherwise
 	 */
-	virtual int disable() { return 0; };
+	virtual int disable()
+	{
+		FlightTask::disable();
+		return 0;
+	};
 
 	/**
 	 * Call regularly in the control loop cycle to execute the task
@@ -73,11 +77,11 @@ public:
 	{
 		FlightTask::update();
 
-		r += _sticks(1) * _deltatime;
+		r += _sticks(0) * _deltatime;
 		r = math::constrain(r, 0.5f, 4.f);
-		v -= _sticks(0) * _deltatime;
+		v -= _sticks(1) * _deltatime;
 		v = math::constrain(v, -7.f, 7.f);
-		altitude += _sticks(3) * _deltatime;
+		altitude += _sticks(2) * _deltatime;
 		altitude = math::constrain(altitude, 2.f, 5.f);
 
 		matrix::Vector2<float> target_to_position = matrix::Vector2f(_position.data()) - matrix::Vector2f();