From 98aee29ddcb4d8f71d8fec2b9c982e2cede743d1 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Sat, 28 Dec 2019 18:41:42 +0100 Subject: [PATCH] VTOL: Manual Pos Flight Task: do not use yawrate sp from weather vane if position isn't locked Signed-off-by: Silvan Fuhrer --- .../tasks/ManualPosition/FlightTaskManualPosition.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 57724e5f33..a59a04227e 100644 --- a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/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 }