mc_pos_control_main - Re-add updateVelocityControllerIO for glitch-free position unlock

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -777,6 +777,10 @@ MulticopterPositionControl::run()
 			// vehicle intention.
 			publish_local_pos_sp(local_pos_sp);
 
+			// 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)
+			_flight_tasks.updateVelocityControllerIO(_control.getVelSp(), local_pos_sp.thrust);
+
 			// Part of landing logic: if ground-contact/maybe landed was detected, turn off
 			// controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.
 			// Note: only adust thrust output if there was not thrust-setpoint demand in D-direction.