FlightTask: move position and stick data subscription into tasks, Orbit introduce variable center position

src/lib/FlightTasks/FlightTasks.hpp

@@ -69,18 +69,6 @@ public:
 		}
 	};
 
-	/**
-	 * Call this function initially to point all tasks to the general input data
-	 */
-	void set_general_input_pointers(vehicle_local_position_s *vehicle_local_position,
-					manual_control_setpoint_s *manual_control_setpoint)
-	{
-		for (int i = 0; i < _task_count; i++) {
-			_tasks[i]->set_vehicle_local_position_pointer(vehicle_local_position);
-			_tasks[i]->set_manual_control_setpoint_pointer(manual_control_setpoint);
-		}
-	};
-
 	/**
 	 * Call this function initially to point all tasks to the general output data
 	 */

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -45,11 +45,9 @@ class FlightTask : public control::SuperBlock
 {
 public:
 	FlightTask(SuperBlock *parent, const char *name) :
-		SuperBlock(parent, name)
-	{
-		_vehicle_position = nullptr;
-		_manual_control_setpoint = nullptr;
-	};
+		SuperBlock(parent, name),
+		_sub_vehicle_local_position(ORB_ID(vehicle_local_position), 0, 0, &getSubscriptions())
+	{ };
 	virtual ~FlightTask() {};
 
 	/**
@@ -80,23 +78,10 @@ public:
 		_deltatime  = math::min((int)hrt_elapsed_time(&_last_time_stamp), _timeout) / 1e6f;
 		_last_time_stamp = hrt_absolute_time();
 		updateSubscriptions();
-		_evaluate_sticks();
 		_evaluate_vehicle_position();
 		return 0;
 	};
 
-	/**
-	 * Set vehicle local position input data pointer
-	 * @param pointer to vehicle local position
-	 */
-	void set_vehicle_local_position_pointer(const vehicle_local_position_s *vehicle_local_position) { _vehicle_position = vehicle_local_position; };
-
-	/**
-	 * Set manual control setpoint input data pointer if it's needed for the task
-	 * @param pointer to manual control setpoint
-	 */
-	void set_manual_control_setpoint_pointer(const manual_control_setpoint_s *manual_control_setpoint) { _manual_control_setpoint = manual_control_setpoint; };
-
 	/**
 	 * Set local position setpoint data pointer if it's needed for the task
 	 * @param pointer to manual control setpoint
@@ -104,12 +89,12 @@ public:
 	void set_vehicle_local_position_setpoint_pointer(vehicle_local_position_setpoint_s *vehicle_position_setpoint) { _vehicle_position_setpoint = vehicle_position_setpoint; };
 
 protected:
+	static constexpr int _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 */
 
-	/* Prepared general inputs for every task */
-	matrix::Vector<float, 4> _sticks;
+	/* Current vehicle position for every task */
 	matrix::Vector3f _position; /*< current vehicle position */
 	matrix::Vector3f _velocity; /*< current vehicle velocity */
 	float _yaw = 0.f;
@@ -156,41 +141,23 @@ protected:
 	};
 
 private:
-	static constexpr int _timeout = 500000; /*< maximal time in us before a loop or data times out */
+	uORB::Subscription<vehicle_local_position_s> _sub_vehicle_local_position;
 
 	hrt_abstime _starting_time_stamp = 0; /*< time stamp when task was activated */
 	hrt_abstime _last_time_stamp = 0; /*< time stamp when task was last updated */
 
-	/* General input that every task has */
-	const vehicle_local_position_s *_vehicle_position;
-	const manual_control_setpoint_s *_manual_control_setpoint;
-
 	/* General output that every task has */
 	vehicle_local_position_setpoint_s *_vehicle_position_setpoint;
 
 	void _evaluate_vehicle_position()
 	{
-		if (_vehicle_position != nullptr && hrt_elapsed_time(&_vehicle_position->timestamp) < _timeout) {
-			_position = matrix::Vector3f(&_vehicle_position->x);
-			_velocity = matrix::Vector3f(&_vehicle_position->vx);
-			_yaw = _vehicle_position->yaw;
+		if (hrt_elapsed_time(&_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;
 
 		} else {
-			_velocity = matrix::Vector3f(); /* default velocity is all zero */
+			_velocity.zero(); /* default velocity is all zero */
 		}
 	}
-
-	void _evaluate_sticks()
-	{
-		if (_manual_control_setpoint != nullptr && hrt_elapsed_time(&_manual_control_setpoint->timestamp) < _timeout) {
-			_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

@@ -49,6 +49,7 @@ class FlightTaskManual : public FlightTask
 public:
 	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),
@@ -103,13 +104,11 @@ public:
 	 */
 	virtual int update()
 	{
-		FlightTask::update();
+		int ret = FlightTask::update();
+		ret += _evaluate_sticks();
 
 		/* prepare stick input */
-		matrix::Vector2f stick_xy; /**< horizontal two dimensional stick input within a unit circle */
-		stick_xy(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get());
-		stick_xy(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get());
-		float stick_z = -math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get());
+		matrix::Vector2f stick_xy(_sticks.data()); /**< horizontal two dimensional stick input within a unit circle */
 
 		const float stick_xy_norm = stick_xy.norm();
 
@@ -119,7 +118,7 @@ public:
 		}
 
 		/* rotate stick input to produce velocity setpoint in NED frame */
-		matrix::Vector3f velocity_setpoint(stick_xy(0), stick_xy(1), stick_z);
+		matrix::Vector3f velocity_setpoint(stick_xy(0), stick_xy(1), _sticks(3));
 		velocity_setpoint = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, get_input_frame_yaw())) * velocity_setpoint;
 
 		/* scale [0,1] length velocity vector to maximal manual speed (in m/s) */
@@ -129,14 +128,14 @@ public:
 		velocity_setpoint = velocity_setpoint.emult(vel_scale);
 
 		/* smooth out velocity setpoint by slewrate and return it */
-		vel_sp_slewrate(velocity_setpoint, stick_xy, stick_z);
+		vel_sp_slewrate(velocity_setpoint, stick_xy, _sticks(3));
 		_set_velocity_setpoint(velocity_setpoint);
 
 		/* handle position and altitude hold */
 		const bool stick_xy_zero = stick_xy_norm <= FLT_EPSILON;
-		const bool stick_z_zero = fabsf(stick_z) <= FLT_EPSILON;
+		const bool stick_z_zero = fabsf(_sticks(3)) <= FLT_EPSILON;
 
-		float velocity_xy_norm = matrix::Vector2f(_velocity._data).norm();
+		float velocity_xy_norm = matrix::Vector2f(_velocity.data()).norm();
 		const bool stopped_xy = (_hold_max_xy.get() < FLT_EPSILON || velocity_xy_norm < _hold_max_xy.get());
 		const bool stopped_z = (_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _hold_max_z.get());
 
@@ -157,16 +156,20 @@ public:
 		}
 
 		_set_position_setpoint(_hold_position);
-		return 0;
+		return ret;
 	};
 
 protected:
+	matrix::Vector<float, 4> _sticks;
+
 	float get_input_frame_yaw()
 	{
 		return _yaw;
 	};
 
 private:
+	uORB::Subscription<manual_control_setpoint_s> _sub_manual_control_setpoint;
+
 	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 */
@@ -178,6 +181,27 @@ private:
 
 	matrix::Vector3f _hold_position; /**< position at which the vehicle stays while the input is zero velocity */
 
+	int _evaluate_sticks()
+	{
+		if (hrt_elapsed_time(&_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] */
+
+			/* apply expo and deadzone */
+			_sticks(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get());
+			_sticks(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get());
+			_sticks(2) = math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get());
+
+			return 0;
+
+		} else {
+			_sticks.zero(); /* default is all zero */
+			return 1;
+		}
+	}
 
 
 	/* --- Acceleration Smoothing --- */

src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp

@@ -41,13 +41,13 @@
 
 #pragma once
 
-#include "FlightTask.hpp"
+#include "FlightTaskManual.hpp"
 
-class FlightTaskOrbit : public FlightTask
+class FlightTaskOrbit : public FlightTaskManual
 {
 public:
 	FlightTaskOrbit(SuperBlock *parent, const char *name) :
-		FlightTask(parent, name)
+		FlightTaskManual(parent, name)
 	{};
 	virtual ~FlightTaskOrbit() {};
 
@@ -60,7 +60,9 @@ public:
 		FlightTask::activate();
 		r = 1.f;
 		v =  0.5f;
-		altitude = 6.f;
+		z = _position(2);
+		_center = matrix::Vector2f(_position.data());
+		_center(0) -= r;
 		return 0;
 	};
 
@@ -80,37 +82,34 @@ public:
 	 */
 	virtual int update()
 	{
-		FlightTask::update();
+		int ret = FlightTaskManual::update();
 
 		r += _sticks(0) * _deltatime;
 		r = math::constrain(r, 1.f, 20.f);
 		v -= _sticks(1) * _deltatime;
 		v = math::constrain(v, -7.f, 7.f);
-		altitude += _sticks(2) * _deltatime;
-		altitude = math::constrain(altitude, 2.f, 5.f);
+		z += _sticks(2) * _deltatime;
 
-		matrix::Vector2<float> target_to_position = matrix::Vector2f(_position.data()) - matrix::Vector2f();
-		// TODO: add local frame target position here
+		matrix::Vector2<float> center_to_position = matrix::Vector2f(_position.data()) - _center;
 
 		/* xy velocity to go around in a circle */
-		matrix::Vector2<float> velocity_xy = matrix::Vector2f(target_to_position(1), -target_to_position(0));
+		matrix::Vector2<float> velocity_xy = matrix::Vector2f(center_to_position(1), -center_to_position(0));
 		velocity_xy.normalize();
 		velocity_xy *= v;
 
 		/* xy velocity adjustment to stay on the radius distance */
-		velocity_xy += (r - target_to_position.norm()) * target_to_position.normalized();
+		velocity_xy += (r - center_to_position.norm()) * center_to_position.normalized();
 
-		//printf("%f %f %f\n", (double)altitude, (double)r, (double)v);
-
-		_set_position_setpoint(matrix::Vector3f(NAN, NAN, -altitude));
+		_set_position_setpoint(matrix::Vector3f(NAN, NAN, z));
 		_set_velocity_setpoint(matrix::Vector3f(velocity_xy(0), velocity_xy(1), 0.f));
-		return 0;
+		return ret;
 	};
 
 private:
 
-	float r = 1.f; /* radius with which to orbit the target */
-	float v =  0.1f; /* linear velocity for orbiting in m/s */
-	float altitude = 2.f; /* altitude in meters */
+	float r = 0.f; /* radius with which to orbit the target */
+	float v =  0.f; /* linear velocity for orbiting in m/s */
+	float z = 0.f; /* local z coordinate in meters */
+	matrix::Vector2f _center;
 
 };