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/FlightModeManager.cpp

@@ -452,18 +452,6 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 {
 	_current_task.task->setYawHandler(_wv_controller);
 
-	// Inform FlightTask 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)
-	if (_vehicle_local_position_setpoint_sub.updated()) {
-		vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
-
-		if (_vehicle_local_position_setpoint_sub.copy(&vehicle_local_position_setpoint)) {
-			const Vector3f vel_sp{vehicle_local_position_setpoint.vx, vehicle_local_position_setpoint.vy, vehicle_local_position_setpoint.vz};
-			const Vector3f acc_sp{vehicle_local_position_setpoint.acceleration};
-			_current_task.task->updateVelocityControllerFeedback(vel_sp, acc_sp);
-		}
-	}
-
 	// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
 	vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
 	vehicle_constraints_s constraints = FlightTask::empty_constraints;

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -144,7 +144,6 @@ private:
 	uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
 	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
 	uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
 	uORB::SubscriptionData<vehicle_control_mode_s> _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::SubscriptionData<vehicle_land_detected_s> _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};

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 */