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;