FlightTasks: replace setpoint setters with members

I realized that instead of using the setpoint setters inline
in real world tasks everyone started to have its own setpoint
member variable and only call the setter in the end for all the
privatly generate setpoints. This makes the setter useless and
therefore I switched to member setpoints in the architecture.
They bring more felxibility which is obviously needed but also
less structure which is the price to pay.

src/lib/FlightTasks/FlightTasks.cpp

@@ -15,7 +15,7 @@ bool FlightTasks::update()
 	return false;
 }
 
-const vehicle_local_position_setpoint_s &FlightTasks::getPositionSetpoint()
+const vehicle_local_position_setpoint_s FlightTasks::getPositionSetpoint()
 {
 	if (isAnyTaskActive()) {
 		return _current_task->getPositionSetpoint();

src/lib/FlightTasks/FlightTasks.hpp

@@ -91,12 +91,7 @@ public:
 	 * Get the output data from the current task
 	 * @return output setpoint, to be executed by position control
 	 */
-	const vehicle_local_position_setpoint_s &getPositionSetpoint();
-
-	/**
-	 * Convenient operator to get the output data from the current task
-	 */
-	inline const vehicle_local_position_setpoint_s &operator()() { return getPositionSetpoint(); }
+	const vehicle_local_position_setpoint_s getPositionSetpoint();
 
 	/**
 	 * Switch to the next task in the available list (for testing)

src/lib/FlightTasks/tasks/FlightTask.cpp

@@ -16,13 +16,13 @@ bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
 
 bool FlightTask::activate()
 {
+	_resetSetpoints();
 	_time_stamp_activate = hrt_absolute_time();
 	return true;
 }
 
 bool FlightTask::updateInitialize()
 {
-	_resetSetpoint();
 	_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;
@@ -30,6 +30,29 @@ bool FlightTask::updateInitialize()
 	return _evaluateVehiclePosition();
 }
 
+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();
+	_position_setpoint.copyToRaw(&vehicle_local_position_setpoint.x);
+	_velocity_setpoint.copyToRaw(&vehicle_local_position_setpoint.vx);
+	_acceleration_setpoint.copyToRaw(&vehicle_local_position_setpoint.acc_x);
+	_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::_evaluateVehiclePosition()
 {
 	if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -56,7 +56,7 @@ class FlightTask : public control::Block
 public:
 	FlightTask(control::SuperBlock *parent, const char *name) :
 		Block(parent, name)
-	{ _resetSetpoint(); }
+	{ _resetSetpoints(); }
 
 	virtual ~FlightTask() = default;
 
@@ -94,10 +94,7 @@ public:
 	/**
 	 * Get the output data
 	 */
-	const vehicle_local_position_setpoint_s &getPositionSetpoint()
-	{
-		return _vehicle_local_position_setpoint;
-	}
+	const vehicle_local_position_setpoint_s getPositionSetpoint();
 
 	static const vehicle_local_position_setpoint_s empty_setpoint;
 
@@ -110,35 +107,27 @@ protected:
 	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 position */
+	/* Current vehicle state */
 	matrix::Vector3f _position; /**< current vehicle position */
 	matrix::Vector3f _velocity; /**< current vehicle velocity */
 	float _yaw = 0.f;
 
-	/* Put the position vector produced by the task into the setpoint message */
-	void _setPositionSetpoint(const matrix::Vector3f &position_setpoint) { position_setpoint.copyToRaw(&_vehicle_local_position_setpoint.x); }
+	/* Setpoints the position controller needs to execute
+	 * NAN values mean the state does not get controlled */
+	matrix::Vector3f _position_setpoint;
+	matrix::Vector3f _velocity_setpoint;
+	matrix::Vector3f _acceleration_setpoint;
+	matrix::Vector3f _thrust_setpoint;
+	float _yaw_setpoint;
+	float _yawspeed_setpoint;
 
-	/* Put the velocity vector produced by the task into the setpoint message */
-	void _setVelocitySetpoint(const matrix::Vector3f &velocity_setpoint) { velocity_setpoint.copyToRaw(&_vehicle_local_position_setpoint.vx); }
-
-	/* Put the acceleration vector produced by the task into the setpoint message */
-	void _setAccelerationSetpoint(const matrix::Vector3f &acceleration_setpoint) { acceleration_setpoint.copyToRaw(&_vehicle_local_position_setpoint.acc_x); }
-
-	/* Put the yaw angle produced by the task into the setpoint message */
-	void _setYawSetpoint(const float yaw) { _vehicle_local_position_setpoint.yaw = yaw; }
-
-	/* Put the yaw anglular rate produced by the task into the setpoint message */
-	void _setYawspeedSetpoint(const float &yawspeed) { _vehicle_local_position_setpoint.yawspeed = yawspeed; }
-
-	/* Put the thrust setpoint produced by the task into the setpoint message */
-	void _setThrustSetpoint(const matrix::Vector3f &thrust_setpoint) { thrust_setpoint.copyTo(_vehicle_local_position_setpoint.thrust); }
+	/**
+	 * Get the output data
+	 */
+	void _resetSetpoints();
 
 private:
 	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 */
-
-	void _resetSetpoint() { _vehicle_local_position_setpoint = empty_setpoint; }
-
 	bool _evaluateVehiclePosition();
 };

src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp

@@ -147,11 +147,11 @@ bool FlightTaskManualStabilized::update()
 	_scaleSticks();
 	_updateSetpoints();
 
-	_setPositionSetpoint(_pos_sp);
-	_setVelocitySetpoint(_vel_sp);
-	_setThrustSetpoint(_thr_sp);
-	_setYawSetpoint(_yaw_sp);
-	_setYawspeedSetpoint(_yaw_rate_sp);
+	_position_setpoint = _pos_sp;
+	_velocity_setpoint = _vel_sp;
+	_thrust_setpoint = _thr_sp;
+	_yaw_setpoint = _yaw_sp;
+	_yawspeed_setpoint = _yaw_rate_sp;
 
 	return true;
 }

src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp

@@ -79,9 +79,10 @@ bool FlightTaskOrbit::update()
 	_v = math::constrain(_v, -7.f, 7.f);
 	_z += _sticks_expo(2) * _deltatime;
 
-	Vector2f center_to_position = Vector2f(_position.data()) - _center;
+	_position_setpoint = Vector3f(NAN, NAN, _z);
 
 	/* xy velocity to go around in a circle */
+	Vector2f center_to_position = Vector2f(_position.data()) - _center;
 	Vector2f velocity_xy = Vector2f(center_to_position(1), -center_to_position(0));
 	velocity_xy = velocity_xy.unit_or_zero();
 	velocity_xy *= _v;
@@ -89,10 +90,9 @@ bool FlightTaskOrbit::update()
 	/* xy velocity adjustment to stay on the radius distance */
 	velocity_xy += (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
 
-	float yaw = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
+	_velocity_setpoint = Vector3f(velocity_xy(0), velocity_xy(1), 0.f);
 
-	_setPositionSetpoint(Vector3f(NAN, NAN, _z));
-	_setVelocitySetpoint(Vector3f(velocity_xy(0), velocity_xy(1), 0.f));
-	_setYawSetpoint(yaw);
+	/* make vehicle front always point towards the center */
+	_yawspeed_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
 	return true;
 }