FlightModeManager: move velocity control feedback into FlightTask

This was only handled outside because FlightTaks lived in the
multicopter position controller which produces the data that was
needed but now it doesn't make sense anymore to handle this
subscription separately.

It's better to have it inside the base task to have the data available
on task activation sucht that e.g. Altitude mode can take over smoothly
from Position mode.

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -34,6 +34,7 @@ bool FlightTask::updateInitialize()
 	_sub_home_position.update();
 
 	_evaluateVehicleLocalPosition();
+	_evaluateVehicleLocalPositionSetpoint();
 	_evaluateDistanceToGround();
 	return true;
 }
@@ -163,6 +164,25 @@ void FlightTask::_evaluateVehicleLocalPosition()
 	}
 }
 
+void FlightTask::_evaluateVehicleLocalPositionSetpoint()
+{
+	vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
+
+	// Only use data that is received within a certain timestamp
+	if (_vehicle_local_position_setpoint_sub.copy(&vehicle_local_position_setpoint)
+	    && (_time_stamp_current - vehicle_local_position_setpoint.timestamp) < _timeout) {
+		// Inform about the input and output of the velocity controller
+		// This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock)
+		_velocity_setpoint_feedback = matrix::Vector3f(vehicle_local_position_setpoint.vx, vehicle_local_position_setpoint.vy,
+					      vehicle_local_position_setpoint.vz);
+		_acceleration_setpoint_feedback = matrix::Vector3f(vehicle_local_position_setpoint.acceleration);
+
+	} else {
+		_velocity_setpoint_feedback.setAll(NAN);
+		_acceleration_setpoint_feedback.setAll(NAN);
+	}
+}
+
 void FlightTask::_evaluateDistanceToGround()
 {
 	// Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -186,13 +186,14 @@ public:
 protected:
 	uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
 	uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
+	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
 
 	/** Reset all setpoints to NAN */
 	void _resetSetpoints();
 
 	/** Check and update local position */
 	void _evaluateVehicleLocalPosition();
-
+	void _evaluateVehicleLocalPositionSetpoint();
 	void _evaluateDistanceToGround();
 
 	/** Set constraints to default values */