From a3710fcdefa2eb109f5250908c7b2ebf0301c9fc Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Sat, 28 Dec 2019 18:42:08 +0100 Subject: [PATCH] VTOL: flight task auto: disable weather vane when not stationary Use yawrate setpoint from weather vane only if velocity setpoint is small (smaller than half the cruise velocity) Signed-off-by: Silvan Fuhrer --- src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp index 650a14c2ea..8c6aa020e6 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp @@ -248,7 +248,18 @@ bool FlightTaskAuto::_evaluateTriplets() // set heading if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { _yaw_setpoint = _yaw; - _yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate(); + // use the yawrate setpoint from WV only if not moving lateral (velocity setpoint below half of _param_mpc_xy_cruise) + // otherwise, keep heading constant (as output from WV is not according to wind in this case) + bool vehicle_is_moving_lateral = _velocity_setpoint.xy().longerThan(_param_mpc_xy_cruise.get() / 2.0f); + + if (vehicle_is_moving_lateral) { + _yawspeed_setpoint = 0.0f; + + } else { + _yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate(); + } + + } else if (_type == WaypointType::follow_target && _sub_triplet_setpoint.get().current.yawspeed_valid) { _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;