VTOL: Manual Pos Flight Task: do not use yawrate sp from weather vane if position isn't locked

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -166,12 +166,19 @@ void FlightTaskManualPosition::_updateSetpoints()
 {
 	FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
 
+	_updateXYlock(); // check for position lock
+
 	// check if an external yaw handler is active and if yes, let it update the yaw setpoints
 	if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) {
 		_yaw_setpoint = NAN;
-		_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
+
+		// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
+		if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
+			// vehicle is steady
+			_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
+		}
+
 	}
 
 	_thrust_setpoint.setAll(NAN); // don't require any thrust setpoints
-	_updateXYlock(); // check for position lock
 }